1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16 package com.irurueta.navigation.inertial.calibration.accelerometer;
17
18 import com.irurueta.algebra.Matrix;
19 import com.irurueta.algebra.WrongSizeException;
20 import com.irurueta.navigation.LockedException;
21 import com.irurueta.navigation.NotReadyException;
22 import com.irurueta.navigation.frames.CoordinateTransformation;
23 import com.irurueta.navigation.frames.ECEFFrame;
24 import com.irurueta.navigation.frames.FrameType;
25 import com.irurueta.navigation.frames.InvalidSourceAndDestinationFrameTypeException;
26 import com.irurueta.navigation.frames.NEDFrame;
27 import com.irurueta.navigation.frames.converters.NEDtoECEFFrameConverter;
28 import com.irurueta.navigation.frames.converters.NEDtoECEFPositionVelocityConverter;
29 import com.irurueta.navigation.geodesic.Constants;
30 import com.irurueta.navigation.inertial.BodyKinematics;
31 import com.irurueta.navigation.inertial.ECEFGravity;
32 import com.irurueta.navigation.inertial.ECEFPosition;
33 import com.irurueta.navigation.inertial.ECEFVelocity;
34 import com.irurueta.navigation.inertial.NEDPosition;
35 import com.irurueta.navigation.inertial.NEDVelocity;
36 import com.irurueta.navigation.inertial.calibration.BodyKinematicsGenerator;
37 import com.irurueta.navigation.inertial.calibration.CalibrationException;
38 import com.irurueta.navigation.inertial.calibration.IMUErrors;
39 import com.irurueta.navigation.inertial.calibration.StandardDeviationBodyKinematics;
40 import com.irurueta.navigation.inertial.estimators.ECEFGravityEstimator;
41 import com.irurueta.navigation.inertial.estimators.ECEFKinematicsEstimator;
42 import com.irurueta.statistics.UniformRandomizer;
43 import com.irurueta.units.Acceleration;
44 import com.irurueta.units.AccelerationUnit;
45 import org.junit.Test;
46
47 import java.util.ArrayList;
48 import java.util.Collection;
49 import java.util.Collections;
50 import java.util.List;
51 import java.util.Random;
52
53 import static org.junit.Assert.*;
54
55 public class KnownPositionAccelerometerCalibratorTest implements
56 KnownPositionAccelerometerCalibratorListener {
57
58 private static final double TIME_INTERVAL_SECONDS = 0.02;
59
60 private static final double MICRO_G_TO_METERS_PER_SECOND_SQUARED = 9.80665E-6;
61 private static final double DEG_TO_RAD = 0.01745329252;
62
63 private static final double MIN_ANGLE_DEGREES = -180.0;
64 private static final double MAX_ANGLE_DEGREES = 180.0;
65
66 private static final double MIN_LATITUDE_DEGREES = -90.0;
67 private static final double MAX_LATITUDE_DEGREEs = 90.0;
68 private static final double MIN_LONGITUDE_DEGREES = -180.0;
69 private static final double MAX_LONGITUDE_DEGREES = 180.0;
70 private static final double MIN_HEIGHT = -50.0;
71 private static final double MAX_HEIGHT = 50.0;
72
73 private static final int LARGE_MEASUREMENT_NUMBER = 100000;
74
75 private static final double ABSOLUTE_ERROR = 1e-8;
76 private static final double LARGE_ABSOLUTE_ERROR = 5e-5;
77 private static final double VERY_LARGE_ABSOLUTE_ERROR = 1e-3;
78
79 private static final int TIMES = 100;
80
81 private int mCalibrateStart;
82 private int mCalibrateEnd;
83
84 @Test
85 public void testConstructor1() throws WrongSizeException {
86 final KnownPositionAccelerometerCalibrator calibrator =
87 new KnownPositionAccelerometerCalibrator();
88
89
90 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
91 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
92 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
93 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
94 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
95 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
96 final Acceleration bx2 = new Acceleration(0.0,
97 AccelerationUnit.FEET_PER_SQUARED_SECOND);
98 calibrator.getInitialBiasXAsAcceleration(bx2);
99 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
100 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
101 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
102 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
103 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
104 final Acceleration by2 = new Acceleration(0.0,
105 AccelerationUnit.FEET_PER_SQUARED_SECOND);
106 calibrator.getInitialBiasYAsAcceleration(by2);
107 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
108 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
109 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
110 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
111 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
112 final Acceleration bz2 = new Acceleration(0.0,
113 AccelerationUnit.FEET_PER_SQUARED_SECOND);
114 calibrator.getInitialBiasZAsAcceleration(bz2);
115 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
116 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
117 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
118 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
119 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
120 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
121 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
122 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
123 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
124 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
125 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
126 final double[] bias1 = calibrator.getInitialBias();
127 assertArrayEquals(bias1, new double[3], 0.0);
128 final double[] bias2 = new double[3];
129 calibrator.getInitialBias(bias2);
130 assertArrayEquals(bias1, bias2, 0.0);
131 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
132 assertEquals(b1, new Matrix(3, 1));
133 final Matrix b2 = new Matrix(3, 1);
134 calibrator.getInitialBiasAsMatrix(b2);
135 assertEquals(b1, b2);
136 final Matrix ma1 = calibrator.getInitialMa();
137 assertEquals(ma1, new Matrix(3, 3));
138 final Matrix ma2 = new Matrix(3, 3);
139 calibrator.getInitialMa(ma2);
140 assertEquals(ma1, ma2);
141 assertNull(calibrator.getEcefPosition());
142 assertNull(calibrator.getNedPosition());
143 assertFalse(calibrator.getNedPosition(null));
144 assertNull(calibrator.getMeasurements());
145 assertFalse(calibrator.isCommonAxisUsed());
146 assertNull(calibrator.getListener());
147 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
148 assertFalse(calibrator.isReady());
149 assertFalse(calibrator.isRunning());
150 assertNull(calibrator.getEstimatedBiases());
151 assertFalse(calibrator.getEstimatedBiases(null));
152 assertNull(calibrator.getEstimatedBiasesAsMatrix());
153 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
154 assertNull(calibrator.getEstimatedBiasFx());
155 assertNull(calibrator.getEstimatedBiasFy());
156 assertNull(calibrator.getEstimatedBiasFz());
157 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
158 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
159 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
160 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
161 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
162 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
163 assertNull(calibrator.getEstimatedMa());
164 assertNull(calibrator.getEstimatedSx());
165 assertNull(calibrator.getEstimatedSy());
166 assertNull(calibrator.getEstimatedSz());
167 assertNull(calibrator.getEstimatedMxy());
168 assertNull(calibrator.getEstimatedMxz());
169 assertNull(calibrator.getEstimatedMyx());
170 assertNull(calibrator.getEstimatedMyz());
171 assertNull(calibrator.getEstimatedMzx());
172 assertNull(calibrator.getEstimatedMzy());
173 assertNull(calibrator.getEstimatedCovariance());
174 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
175 assertNull(calibrator.getGroundTruthGravityNorm());
176 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
177 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
178 }
179
180 @Test
181 public void testConstructor2() throws WrongSizeException {
182 final KnownPositionAccelerometerCalibrator calibrator =
183 new KnownPositionAccelerometerCalibrator(this);
184
185
186 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
187 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
188 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
189 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
190 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
191 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
192 final Acceleration bx2 = new Acceleration(0.0,
193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
194 calibrator.getInitialBiasXAsAcceleration(bx2);
195 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
196 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
197 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
198 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
199 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
200 final Acceleration by2 = new Acceleration(0.0,
201 AccelerationUnit.FEET_PER_SQUARED_SECOND);
202 calibrator.getInitialBiasYAsAcceleration(by2);
203 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
204 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
205 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
206 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
207 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
208 final Acceleration bz2 = new Acceleration(0.0,
209 AccelerationUnit.FEET_PER_SQUARED_SECOND);
210 calibrator.getInitialBiasZAsAcceleration(bz2);
211 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
212 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
213 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
214 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
215 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
216 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
217 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
218 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
219 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
220 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
221 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
222 final double[] bias1 = calibrator.getInitialBias();
223 assertArrayEquals(bias1, new double[3], 0.0);
224 final double[] bias2 = new double[3];
225 calibrator.getInitialBias(bias2);
226 assertArrayEquals(bias1, bias2, 0.0);
227 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
228 assertEquals(b1, new Matrix(3, 1));
229 final Matrix b2 = new Matrix(3, 1);
230 calibrator.getInitialBiasAsMatrix(b2);
231 assertEquals(b1, b2);
232 final Matrix ma1 = calibrator.getInitialMa();
233 assertEquals(ma1, new Matrix(3, 3));
234 final Matrix ma2 = new Matrix(3, 3);
235 calibrator.getInitialMa(ma2);
236 assertEquals(ma1, ma2);
237 assertNull(calibrator.getEcefPosition());
238 assertNull(calibrator.getNedPosition());
239 assertFalse(calibrator.getNedPosition(null));
240 assertNull(calibrator.getMeasurements());
241 assertFalse(calibrator.isCommonAxisUsed());
242 assertSame(calibrator.getListener(), this);
243 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
244 assertFalse(calibrator.isReady());
245 assertFalse(calibrator.isRunning());
246 assertNull(calibrator.getEstimatedBiases());
247 assertFalse(calibrator.getEstimatedBiases(null));
248 assertNull(calibrator.getEstimatedBiasesAsMatrix());
249 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
250 assertNull(calibrator.getEstimatedBiasFx());
251 assertNull(calibrator.getEstimatedBiasFy());
252 assertNull(calibrator.getEstimatedBiasFz());
253 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
254 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
255 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
256 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
257 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
258 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
259 assertNull(calibrator.getEstimatedMa());
260 assertNull(calibrator.getEstimatedSx());
261 assertNull(calibrator.getEstimatedSy());
262 assertNull(calibrator.getEstimatedSz());
263 assertNull(calibrator.getEstimatedMxy());
264 assertNull(calibrator.getEstimatedMxz());
265 assertNull(calibrator.getEstimatedMyx());
266 assertNull(calibrator.getEstimatedMyz());
267 assertNull(calibrator.getEstimatedMzx());
268 assertNull(calibrator.getEstimatedMzy());
269 assertNull(calibrator.getEstimatedCovariance());
270 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
271 assertNull(calibrator.getGroundTruthGravityNorm());
272 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
273 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
274 }
275
276 @Test
277 public void testConstructor3() throws WrongSizeException {
278 final Collection<StandardDeviationBodyKinematics> measurements =
279 Collections.emptyList();
280 final KnownPositionAccelerometerCalibrator calibrator =
281 new KnownPositionAccelerometerCalibrator(measurements);
282
283
284 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
285 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
286 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
287 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
288 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
289 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
290 final Acceleration bx2 = new Acceleration(0.0,
291 AccelerationUnit.FEET_PER_SQUARED_SECOND);
292 calibrator.getInitialBiasXAsAcceleration(bx2);
293 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
294 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
295 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
296 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
297 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
298 final Acceleration by2 = new Acceleration(0.0,
299 AccelerationUnit.FEET_PER_SQUARED_SECOND);
300 calibrator.getInitialBiasYAsAcceleration(by2);
301 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
302 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
303 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
304 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
305 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
306 final Acceleration bz2 = new Acceleration(0.0,
307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
308 calibrator.getInitialBiasZAsAcceleration(bz2);
309 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
310 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
311 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
312 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
313 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
314 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
315 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
316 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
317 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
318 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
319 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
320 final double[] bias1 = calibrator.getInitialBias();
321 assertArrayEquals(bias1, new double[3], 0.0);
322 final double[] bias2 = new double[3];
323 calibrator.getInitialBias(bias2);
324 assertArrayEquals(bias1, bias2, 0.0);
325 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
326 assertEquals(b1, new Matrix(3, 1));
327 final Matrix b2 = new Matrix(3, 1);
328 calibrator.getInitialBiasAsMatrix(b2);
329 assertEquals(b1, b2);
330 final Matrix ma1 = calibrator.getInitialMa();
331 assertEquals(ma1, new Matrix(3, 3));
332 final Matrix ma2 = new Matrix(3, 3);
333 calibrator.getInitialMa(ma2);
334 assertEquals(ma1, ma2);
335 assertNull(calibrator.getEcefPosition());
336 assertNull(calibrator.getNedPosition());
337 assertFalse(calibrator.getNedPosition(null));
338 assertSame(calibrator.getMeasurements(), measurements);
339 assertFalse(calibrator.isCommonAxisUsed());
340 assertNull(calibrator.getListener());
341 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
342 assertFalse(calibrator.isReady());
343 assertFalse(calibrator.isRunning());
344 assertNull(calibrator.getEstimatedBiases());
345 assertFalse(calibrator.getEstimatedBiases(null));
346 assertNull(calibrator.getEstimatedBiasesAsMatrix());
347 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
348 assertNull(calibrator.getEstimatedBiasFx());
349 assertNull(calibrator.getEstimatedBiasFy());
350 assertNull(calibrator.getEstimatedBiasFz());
351 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
352 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
353 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
354 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
355 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
356 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
357 assertNull(calibrator.getEstimatedMa());
358 assertNull(calibrator.getEstimatedSx());
359 assertNull(calibrator.getEstimatedSy());
360 assertNull(calibrator.getEstimatedSz());
361 assertNull(calibrator.getEstimatedMxy());
362 assertNull(calibrator.getEstimatedMxz());
363 assertNull(calibrator.getEstimatedMyx());
364 assertNull(calibrator.getEstimatedMyz());
365 assertNull(calibrator.getEstimatedMzx());
366 assertNull(calibrator.getEstimatedMzy());
367 assertNull(calibrator.getEstimatedCovariance());
368 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
369 assertNull(calibrator.getGroundTruthGravityNorm());
370 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
371 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
372 }
373
374 @Test
375 public void testConstructor4() throws WrongSizeException {
376 final Collection<StandardDeviationBodyKinematics> measurements =
377 Collections.emptyList();
378 final KnownPositionAccelerometerCalibrator calibrator =
379 new KnownPositionAccelerometerCalibrator(measurements, this);
380
381
382 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
383 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
384 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
385 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
386 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
387 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
388 final Acceleration bx2 = new Acceleration(0.0,
389 AccelerationUnit.FEET_PER_SQUARED_SECOND);
390 calibrator.getInitialBiasXAsAcceleration(bx2);
391 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
392 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
393 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
394 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
395 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
396 final Acceleration by2 = new Acceleration(0.0,
397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
398 calibrator.getInitialBiasYAsAcceleration(by2);
399 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
400 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
401 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
402 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
403 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
404 final Acceleration bz2 = new Acceleration(0.0,
405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
406 calibrator.getInitialBiasZAsAcceleration(bz2);
407 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
408 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
409 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
410 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
411 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
412 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
413 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
414 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
415 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
416 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
417 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
418 final double[] bias1 = calibrator.getInitialBias();
419 assertArrayEquals(bias1, new double[3], 0.0);
420 final double[] bias2 = new double[3];
421 calibrator.getInitialBias(bias2);
422 assertArrayEquals(bias1, bias2, 0.0);
423 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
424 assertEquals(b1, new Matrix(3, 1));
425 final Matrix b2 = new Matrix(3, 1);
426 calibrator.getInitialBiasAsMatrix(b2);
427 assertEquals(b1, b2);
428 final Matrix ma1 = calibrator.getInitialMa();
429 assertEquals(ma1, new Matrix(3, 3));
430 final Matrix ma2 = new Matrix(3, 3);
431 calibrator.getInitialMa(ma2);
432 assertEquals(ma1, ma2);
433 assertNull(calibrator.getEcefPosition());
434 assertNull(calibrator.getNedPosition());
435 assertFalse(calibrator.getNedPosition(null));
436 assertSame(calibrator.getMeasurements(), measurements);
437 assertFalse(calibrator.isCommonAxisUsed());
438 assertSame(calibrator.getListener(), this);
439 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
440 assertFalse(calibrator.isReady());
441 assertFalse(calibrator.isRunning());
442 assertNull(calibrator.getEstimatedBiases());
443 assertFalse(calibrator.getEstimatedBiases(null));
444 assertNull(calibrator.getEstimatedBiasesAsMatrix());
445 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
446 assertNull(calibrator.getEstimatedBiasFx());
447 assertNull(calibrator.getEstimatedBiasFy());
448 assertNull(calibrator.getEstimatedBiasFz());
449 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
450 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
451 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
452 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
453 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
454 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
455 assertNull(calibrator.getEstimatedMa());
456 assertNull(calibrator.getEstimatedSx());
457 assertNull(calibrator.getEstimatedSy());
458 assertNull(calibrator.getEstimatedSz());
459 assertNull(calibrator.getEstimatedMxy());
460 assertNull(calibrator.getEstimatedMxz());
461 assertNull(calibrator.getEstimatedMyx());
462 assertNull(calibrator.getEstimatedMyz());
463 assertNull(calibrator.getEstimatedMzx());
464 assertNull(calibrator.getEstimatedMzy());
465 assertNull(calibrator.getEstimatedCovariance());
466 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
467 assertNull(calibrator.getGroundTruthGravityNorm());
468 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
469 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
470 }
471
472 @Test
473 public void testConstructor5() throws WrongSizeException {
474 final KnownPositionAccelerometerCalibrator calibrator =
475 new KnownPositionAccelerometerCalibrator(true);
476
477
478 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
479 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
480 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
481 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
482 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
483 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
484 final Acceleration bx2 = new Acceleration(0.0,
485 AccelerationUnit.FEET_PER_SQUARED_SECOND);
486 calibrator.getInitialBiasXAsAcceleration(bx2);
487 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
488 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
489 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
490 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
491 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
492 final Acceleration by2 = new Acceleration(0.0,
493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
494 calibrator.getInitialBiasYAsAcceleration(by2);
495 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
496 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
497 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
498 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
499 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
500 final Acceleration bz2 = new Acceleration(0.0,
501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
502 calibrator.getInitialBiasZAsAcceleration(bz2);
503 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
504 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
505 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
506 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
507 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
508 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
509 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
510 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
511 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
512 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
513 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
514 final double[] bias1 = calibrator.getInitialBias();
515 assertArrayEquals(bias1, new double[3], 0.0);
516 final double[] bias2 = new double[3];
517 calibrator.getInitialBias(bias2);
518 assertArrayEquals(bias1, bias2, 0.0);
519 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
520 assertEquals(b1, new Matrix(3, 1));
521 final Matrix b2 = new Matrix(3, 1);
522 calibrator.getInitialBiasAsMatrix(b2);
523 assertEquals(b1, b2);
524 final Matrix ma1 = calibrator.getInitialMa();
525 assertEquals(ma1, new Matrix(3, 3));
526 final Matrix ma2 = new Matrix(3, 3);
527 calibrator.getInitialMa(ma2);
528 assertEquals(ma1, ma2);
529 assertNull(calibrator.getEcefPosition());
530 assertNull(calibrator.getNedPosition());
531 assertFalse(calibrator.getNedPosition(null));
532 assertNull(calibrator.getMeasurements());
533 assertTrue(calibrator.isCommonAxisUsed());
534 assertNull(calibrator.getListener());
535 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
536 assertFalse(calibrator.isReady());
537 assertFalse(calibrator.isRunning());
538 assertNull(calibrator.getEstimatedBiases());
539 assertFalse(calibrator.getEstimatedBiases(null));
540 assertNull(calibrator.getEstimatedBiasesAsMatrix());
541 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
542 assertNull(calibrator.getEstimatedBiasFx());
543 assertNull(calibrator.getEstimatedBiasFy());
544 assertNull(calibrator.getEstimatedBiasFz());
545 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
546 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
547 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
548 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
549 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
550 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
551 assertNull(calibrator.getEstimatedMa());
552 assertNull(calibrator.getEstimatedSx());
553 assertNull(calibrator.getEstimatedSy());
554 assertNull(calibrator.getEstimatedSz());
555 assertNull(calibrator.getEstimatedMxy());
556 assertNull(calibrator.getEstimatedMxz());
557 assertNull(calibrator.getEstimatedMyx());
558 assertNull(calibrator.getEstimatedMyz());
559 assertNull(calibrator.getEstimatedMzx());
560 assertNull(calibrator.getEstimatedMzy());
561 assertNull(calibrator.getEstimatedCovariance());
562 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
563 assertNull(calibrator.getGroundTruthGravityNorm());
564 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
565 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
566 }
567
568 @Test
569 public void testConstructor6() throws WrongSizeException {
570 final KnownPositionAccelerometerCalibrator calibrator =
571 new KnownPositionAccelerometerCalibrator(true,
572 this);
573
574
575 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
576 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
577 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
578 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
579 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
580 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
581 final Acceleration bx2 = new Acceleration(0.0,
582 AccelerationUnit.FEET_PER_SQUARED_SECOND);
583 calibrator.getInitialBiasXAsAcceleration(bx2);
584 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
585 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
586 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
587 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
588 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
589 final Acceleration by2 = new Acceleration(0.0,
590 AccelerationUnit.FEET_PER_SQUARED_SECOND);
591 calibrator.getInitialBiasYAsAcceleration(by2);
592 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
593 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
594 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
595 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
596 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
597 final Acceleration bz2 = new Acceleration(0.0,
598 AccelerationUnit.FEET_PER_SQUARED_SECOND);
599 calibrator.getInitialBiasZAsAcceleration(bz2);
600 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
601 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
602 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
603 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
604 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
605 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
606 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
607 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
608 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
609 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
610 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
611 final double[] bias1 = calibrator.getInitialBias();
612 assertArrayEquals(bias1, new double[3], 0.0);
613 final double[] bias2 = new double[3];
614 calibrator.getInitialBias(bias2);
615 assertArrayEquals(bias1, bias2, 0.0);
616 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
617 assertEquals(b1, new Matrix(3, 1));
618 final Matrix b2 = new Matrix(3, 1);
619 calibrator.getInitialBiasAsMatrix(b2);
620 assertEquals(b1, b2);
621 final Matrix ma1 = calibrator.getInitialMa();
622 assertEquals(ma1, new Matrix(3, 3));
623 final Matrix ma2 = new Matrix(3, 3);
624 calibrator.getInitialMa(ma2);
625 assertEquals(ma1, ma2);
626 assertNull(calibrator.getEcefPosition());
627 assertNull(calibrator.getNedPosition());
628 assertFalse(calibrator.getNedPosition(null));
629 assertNull(calibrator.getMeasurements());
630 assertTrue(calibrator.isCommonAxisUsed());
631 assertSame(calibrator.getListener(), this);
632 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
633 assertFalse(calibrator.isReady());
634 assertFalse(calibrator.isRunning());
635 assertNull(calibrator.getEstimatedBiases());
636 assertFalse(calibrator.getEstimatedBiases(null));
637 assertNull(calibrator.getEstimatedBiasesAsMatrix());
638 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
639 assertNull(calibrator.getEstimatedBiasFx());
640 assertNull(calibrator.getEstimatedBiasFy());
641 assertNull(calibrator.getEstimatedBiasFz());
642 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
643 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
644 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
645 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
646 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
647 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
648 assertNull(calibrator.getEstimatedMa());
649 assertNull(calibrator.getEstimatedSx());
650 assertNull(calibrator.getEstimatedSy());
651 assertNull(calibrator.getEstimatedSz());
652 assertNull(calibrator.getEstimatedMxy());
653 assertNull(calibrator.getEstimatedMxz());
654 assertNull(calibrator.getEstimatedMyx());
655 assertNull(calibrator.getEstimatedMyz());
656 assertNull(calibrator.getEstimatedMzx());
657 assertNull(calibrator.getEstimatedMzy());
658 assertNull(calibrator.getEstimatedCovariance());
659 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
660 assertNull(calibrator.getGroundTruthGravityNorm());
661 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
662 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
663 }
664
665 @Test
666 public void testConstructor7() throws WrongSizeException {
667 final Collection<StandardDeviationBodyKinematics> measurements =
668 Collections.emptyList();
669 final KnownPositionAccelerometerCalibrator calibrator =
670 new KnownPositionAccelerometerCalibrator(measurements,
671 true);
672
673
674 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
675 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
676 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
677 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
678 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
679 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
680 final Acceleration bx2 = new Acceleration(0.0,
681 AccelerationUnit.FEET_PER_SQUARED_SECOND);
682 calibrator.getInitialBiasXAsAcceleration(bx2);
683 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
684 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
685 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
686 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
687 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
688 final Acceleration by2 = new Acceleration(0.0,
689 AccelerationUnit.FEET_PER_SQUARED_SECOND);
690 calibrator.getInitialBiasYAsAcceleration(by2);
691 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
692 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
693 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
694 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
695 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
696 final Acceleration bz2 = new Acceleration(0.0,
697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
698 calibrator.getInitialBiasZAsAcceleration(bz2);
699 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
700 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
701 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
702 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
703 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
704 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
705 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
706 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
707 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
708 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
709 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
710 final double[] bias1 = calibrator.getInitialBias();
711 assertArrayEquals(bias1, new double[3], 0.0);
712 final double[] bias2 = new double[3];
713 calibrator.getInitialBias(bias2);
714 assertArrayEquals(bias1, bias2, 0.0);
715 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
716 assertEquals(b1, new Matrix(3, 1));
717 final Matrix b2 = new Matrix(3, 1);
718 calibrator.getInitialBiasAsMatrix(b2);
719 assertEquals(b1, b2);
720 final Matrix ma1 = calibrator.getInitialMa();
721 assertEquals(ma1, new Matrix(3, 3));
722 final Matrix ma2 = new Matrix(3, 3);
723 calibrator.getInitialMa(ma2);
724 assertEquals(ma1, ma2);
725 assertNull(calibrator.getEcefPosition());
726 assertNull(calibrator.getNedPosition());
727 assertFalse(calibrator.getNedPosition(null));
728 assertSame(calibrator.getMeasurements(), measurements);
729 assertTrue(calibrator.isCommonAxisUsed());
730 assertNull(calibrator.getListener());
731 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
732 assertFalse(calibrator.isReady());
733 assertFalse(calibrator.isRunning());
734 assertNull(calibrator.getEstimatedBiases());
735 assertFalse(calibrator.getEstimatedBiases(null));
736 assertNull(calibrator.getEstimatedBiasesAsMatrix());
737 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
738 assertNull(calibrator.getEstimatedBiasFx());
739 assertNull(calibrator.getEstimatedBiasFy());
740 assertNull(calibrator.getEstimatedBiasFz());
741 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
742 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
743 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
744 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
745 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
746 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
747 assertNull(calibrator.getEstimatedMa());
748 assertNull(calibrator.getEstimatedSx());
749 assertNull(calibrator.getEstimatedSy());
750 assertNull(calibrator.getEstimatedSz());
751 assertNull(calibrator.getEstimatedMxy());
752 assertNull(calibrator.getEstimatedMxz());
753 assertNull(calibrator.getEstimatedMyx());
754 assertNull(calibrator.getEstimatedMyz());
755 assertNull(calibrator.getEstimatedMzx());
756 assertNull(calibrator.getEstimatedMzy());
757 assertNull(calibrator.getEstimatedCovariance());
758 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
759 assertNull(calibrator.getGroundTruthGravityNorm());
760 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
761 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
762 }
763
764 @Test
765 public void testConstructor8() throws WrongSizeException {
766 final Collection<StandardDeviationBodyKinematics> measurements =
767 Collections.emptyList();
768 final KnownPositionAccelerometerCalibrator calibrator =
769 new KnownPositionAccelerometerCalibrator(measurements,
770 true, this);
771
772
773 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
774 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
775 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
776 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
777 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
778 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
779 final Acceleration bx2 = new Acceleration(0.0,
780 AccelerationUnit.FEET_PER_SQUARED_SECOND);
781 calibrator.getInitialBiasXAsAcceleration(bx2);
782 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
783 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
784 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
785 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
786 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
787 final Acceleration by2 = new Acceleration(0.0,
788 AccelerationUnit.FEET_PER_SQUARED_SECOND);
789 calibrator.getInitialBiasYAsAcceleration(by2);
790 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
791 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
792 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
793 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
794 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
795 final Acceleration bz2 = new Acceleration(0.0,
796 AccelerationUnit.FEET_PER_SQUARED_SECOND);
797 calibrator.getInitialBiasZAsAcceleration(bz2);
798 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
799 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
800 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
801 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
802 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
803 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
804 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
805 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
806 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
807 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
808 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
809 final double[] bias1 = calibrator.getInitialBias();
810 assertArrayEquals(bias1, new double[3], 0.0);
811 final double[] bias2 = new double[3];
812 calibrator.getInitialBias(bias2);
813 assertArrayEquals(bias1, bias2, 0.0);
814 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
815 assertEquals(b1, new Matrix(3, 1));
816 final Matrix b2 = new Matrix(3, 1);
817 calibrator.getInitialBiasAsMatrix(b2);
818 assertEquals(b1, b2);
819 final Matrix ma1 = calibrator.getInitialMa();
820 assertEquals(ma1, new Matrix(3, 3));
821 final Matrix ma2 = new Matrix(3, 3);
822 calibrator.getInitialMa(ma2);
823 assertEquals(ma1, ma2);
824 assertNull(calibrator.getEcefPosition());
825 assertNull(calibrator.getNedPosition());
826 assertFalse(calibrator.getNedPosition(null));
827 assertSame(calibrator.getMeasurements(), measurements);
828 assertTrue(calibrator.isCommonAxisUsed());
829 assertSame(calibrator.getListener(), this);
830 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
831 assertFalse(calibrator.isReady());
832 assertFalse(calibrator.isRunning());
833 assertNull(calibrator.getEstimatedBiases());
834 assertFalse(calibrator.getEstimatedBiases(null));
835 assertNull(calibrator.getEstimatedBiasesAsMatrix());
836 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
837 assertNull(calibrator.getEstimatedBiasFx());
838 assertNull(calibrator.getEstimatedBiasFy());
839 assertNull(calibrator.getEstimatedBiasFz());
840 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
841 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
842 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
843 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
844 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
845 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
846 assertNull(calibrator.getEstimatedMa());
847 assertNull(calibrator.getEstimatedSx());
848 assertNull(calibrator.getEstimatedSy());
849 assertNull(calibrator.getEstimatedSz());
850 assertNull(calibrator.getEstimatedMxy());
851 assertNull(calibrator.getEstimatedMxz());
852 assertNull(calibrator.getEstimatedMyx());
853 assertNull(calibrator.getEstimatedMyz());
854 assertNull(calibrator.getEstimatedMzx());
855 assertNull(calibrator.getEstimatedMzy());
856 assertNull(calibrator.getEstimatedCovariance());
857 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
858 assertNull(calibrator.getGroundTruthGravityNorm());
859 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
860 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
861 }
862
863 @Test
864 public void testConstructor9() throws WrongSizeException {
865 final Matrix ba = generateBa();
866 final double biasX = ba.getElementAtIndex(0);
867 final double biasY = ba.getElementAtIndex(1);
868 final double biasZ = ba.getElementAtIndex(2);
869
870 final KnownPositionAccelerometerCalibrator calibrator =
871 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ);
872
873
874 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
875 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
876 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
877 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
878 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
879 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
880 final Acceleration bx2 = new Acceleration(0.0,
881 AccelerationUnit.FEET_PER_SQUARED_SECOND);
882 calibrator.getInitialBiasXAsAcceleration(bx2);
883 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
884 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
885 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
886 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
887 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
888 final Acceleration by2 = new Acceleration(0.0,
889 AccelerationUnit.FEET_PER_SQUARED_SECOND);
890 calibrator.getInitialBiasYAsAcceleration(by2);
891 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
892 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
893 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
894 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
895 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
896 final Acceleration bz2 = new Acceleration(0.0,
897 AccelerationUnit.FEET_PER_SQUARED_SECOND);
898 calibrator.getInitialBiasZAsAcceleration(bz2);
899 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
900 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
901 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
902 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
903 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
904 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
905 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
906 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
907 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
908 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
909 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
910 final double[] bias1 = calibrator.getInitialBias();
911 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
912 final double[] bias2 = new double[3];
913 calibrator.getInitialBias(bias2);
914 assertArrayEquals(bias1, bias2, 0.0);
915 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
916 assertEquals(b1, ba);
917 final Matrix b2 = new Matrix(3, 1);
918 calibrator.getInitialBiasAsMatrix(b2);
919 assertEquals(b1, b2);
920 final Matrix ma1 = calibrator.getInitialMa();
921 assertEquals(ma1, new Matrix(3, 3));
922 final Matrix ma2 = new Matrix(3, 3);
923 calibrator.getInitialMa(ma2);
924 assertEquals(ma1, ma2);
925 assertNull(calibrator.getEcefPosition());
926 assertNull(calibrator.getNedPosition());
927 assertFalse(calibrator.getNedPosition(null));
928 assertNull(calibrator.getMeasurements());
929 assertFalse(calibrator.isCommonAxisUsed());
930 assertNull(calibrator.getListener());
931 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
932 assertFalse(calibrator.isReady());
933 assertFalse(calibrator.isRunning());
934 assertNull(calibrator.getEstimatedBiases());
935 assertFalse(calibrator.getEstimatedBiases(null));
936 assertNull(calibrator.getEstimatedBiasesAsMatrix());
937 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
938 assertNull(calibrator.getEstimatedBiasFx());
939 assertNull(calibrator.getEstimatedBiasFy());
940 assertNull(calibrator.getEstimatedBiasFz());
941 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
942 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
943 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
944 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
945 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
946 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
947 assertNull(calibrator.getEstimatedMa());
948 assertNull(calibrator.getEstimatedSx());
949 assertNull(calibrator.getEstimatedSy());
950 assertNull(calibrator.getEstimatedSz());
951 assertNull(calibrator.getEstimatedMxy());
952 assertNull(calibrator.getEstimatedMxz());
953 assertNull(calibrator.getEstimatedMyx());
954 assertNull(calibrator.getEstimatedMyz());
955 assertNull(calibrator.getEstimatedMzx());
956 assertNull(calibrator.getEstimatedMzy());
957 assertNull(calibrator.getEstimatedCovariance());
958 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
959 assertNull(calibrator.getGroundTruthGravityNorm());
960 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
961 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
962 }
963
964 @Test
965 public void testConstructor10() throws WrongSizeException {
966 final Matrix ba = generateBa();
967 final double biasX = ba.getElementAtIndex(0);
968 final double biasY = ba.getElementAtIndex(1);
969 final double biasZ = ba.getElementAtIndex(2);
970
971 final KnownPositionAccelerometerCalibrator calibrator =
972 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
973 this);
974
975
976 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
977 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
978 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
979 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
980 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
981 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
982 final Acceleration bx2 = new Acceleration(0.0,
983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
984 calibrator.getInitialBiasXAsAcceleration(bx2);
985 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
986 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
987 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
988 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
989 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
990 final Acceleration by2 = new Acceleration(0.0,
991 AccelerationUnit.FEET_PER_SQUARED_SECOND);
992 calibrator.getInitialBiasYAsAcceleration(by2);
993 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
994 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
995 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
996 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
997 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
998 final Acceleration bz2 = new Acceleration(0.0,
999 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1000 calibrator.getInitialBiasZAsAcceleration(bz2);
1001 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1002 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1003 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1004 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1005 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1006 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1007 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1008 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1009 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1010 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1011 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1012 final double[] bias1 = calibrator.getInitialBias();
1013 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1014 final double[] bias2 = new double[3];
1015 calibrator.getInitialBias(bias2);
1016 assertArrayEquals(bias1, bias2, 0.0);
1017 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1018 assertEquals(b1, ba);
1019 final Matrix b2 = new Matrix(3, 1);
1020 calibrator.getInitialBiasAsMatrix(b2);
1021 assertEquals(b1, b2);
1022 final Matrix ma1 = calibrator.getInitialMa();
1023 assertEquals(ma1, new Matrix(3, 3));
1024 final Matrix ma2 = new Matrix(3, 3);
1025 calibrator.getInitialMa(ma2);
1026 assertEquals(ma1, ma2);
1027 assertNull(calibrator.getEcefPosition());
1028 assertNull(calibrator.getNedPosition());
1029 assertFalse(calibrator.getNedPosition(null));
1030 assertNull(calibrator.getMeasurements());
1031 assertFalse(calibrator.isCommonAxisUsed());
1032 assertSame(calibrator.getListener(), this);
1033 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1034 assertFalse(calibrator.isReady());
1035 assertFalse(calibrator.isRunning());
1036 assertNull(calibrator.getEstimatedBiases());
1037 assertFalse(calibrator.getEstimatedBiases(null));
1038 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1039 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1040 assertNull(calibrator.getEstimatedBiasFx());
1041 assertNull(calibrator.getEstimatedBiasFy());
1042 assertNull(calibrator.getEstimatedBiasFz());
1043 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1044 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1045 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1046 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1047 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1048 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1049 assertNull(calibrator.getEstimatedMa());
1050 assertNull(calibrator.getEstimatedSx());
1051 assertNull(calibrator.getEstimatedSy());
1052 assertNull(calibrator.getEstimatedSz());
1053 assertNull(calibrator.getEstimatedMxy());
1054 assertNull(calibrator.getEstimatedMxz());
1055 assertNull(calibrator.getEstimatedMyx());
1056 assertNull(calibrator.getEstimatedMyz());
1057 assertNull(calibrator.getEstimatedMzx());
1058 assertNull(calibrator.getEstimatedMzy());
1059 assertNull(calibrator.getEstimatedCovariance());
1060 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1061 assertNull(calibrator.getGroundTruthGravityNorm());
1062 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1063 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1064 }
1065
1066 @Test
1067 public void testConstructor11() throws WrongSizeException {
1068 final Collection<StandardDeviationBodyKinematics> measurements =
1069 Collections.emptyList();
1070
1071 final Matrix ba = generateBa();
1072 final double biasX = ba.getElementAtIndex(0);
1073 final double biasY = ba.getElementAtIndex(1);
1074 final double biasZ = ba.getElementAtIndex(2);
1075
1076 final KnownPositionAccelerometerCalibrator calibrator =
1077 new KnownPositionAccelerometerCalibrator(measurements,
1078 biasX, biasY, biasZ);
1079
1080
1081 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1082 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1083 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1084 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1085 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1086 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1087 final Acceleration bx2 = new Acceleration(0.0,
1088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1089 calibrator.getInitialBiasXAsAcceleration(bx2);
1090 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1091 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1092 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1093 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1094 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1095 final Acceleration by2 = new Acceleration(0.0,
1096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1097 calibrator.getInitialBiasYAsAcceleration(by2);
1098 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1099 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1100 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1101 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1102 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1103 final Acceleration bz2 = new Acceleration(0.0,
1104 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1105 calibrator.getInitialBiasZAsAcceleration(bz2);
1106 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1107 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1108 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1109 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1110 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1111 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1112 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1113 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1114 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1115 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1116 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1117 final double[] bias1 = calibrator.getInitialBias();
1118 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1119 final double[] bias2 = new double[3];
1120 calibrator.getInitialBias(bias2);
1121 assertArrayEquals(bias1, bias2, 0.0);
1122 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1123 assertEquals(b1, ba);
1124 final Matrix b2 = new Matrix(3, 1);
1125 calibrator.getInitialBiasAsMatrix(b2);
1126 assertEquals(b1, b2);
1127 final Matrix ma1 = calibrator.getInitialMa();
1128 assertEquals(ma1, new Matrix(3, 3));
1129 final Matrix ma2 = new Matrix(3, 3);
1130 calibrator.getInitialMa(ma2);
1131 assertEquals(ma1, ma2);
1132 assertNull(calibrator.getEcefPosition());
1133 assertNull(calibrator.getNedPosition());
1134 assertFalse(calibrator.getNedPosition(null));
1135 assertSame(calibrator.getMeasurements(), measurements);
1136 assertFalse(calibrator.isCommonAxisUsed());
1137 assertNull(calibrator.getListener());
1138 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1139 assertFalse(calibrator.isReady());
1140 assertFalse(calibrator.isRunning());
1141 assertNull(calibrator.getEstimatedBiases());
1142 assertFalse(calibrator.getEstimatedBiases(null));
1143 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1144 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1145 assertNull(calibrator.getEstimatedBiasFx());
1146 assertNull(calibrator.getEstimatedBiasFy());
1147 assertNull(calibrator.getEstimatedBiasFz());
1148 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1149 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1150 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1151 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1152 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1153 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1154 assertNull(calibrator.getEstimatedMa());
1155 assertNull(calibrator.getEstimatedSx());
1156 assertNull(calibrator.getEstimatedSy());
1157 assertNull(calibrator.getEstimatedSz());
1158 assertNull(calibrator.getEstimatedMxy());
1159 assertNull(calibrator.getEstimatedMxz());
1160 assertNull(calibrator.getEstimatedMyx());
1161 assertNull(calibrator.getEstimatedMyz());
1162 assertNull(calibrator.getEstimatedMzx());
1163 assertNull(calibrator.getEstimatedMzy());
1164 assertNull(calibrator.getEstimatedCovariance());
1165 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1166 assertNull(calibrator.getGroundTruthGravityNorm());
1167 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1168 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1169 }
1170
1171 @Test
1172 public void testConstructor12() throws WrongSizeException {
1173 final Collection<StandardDeviationBodyKinematics> measurements =
1174 Collections.emptyList();
1175
1176 final Matrix ba = generateBa();
1177 final double biasX = ba.getElementAtIndex(0);
1178 final double biasY = ba.getElementAtIndex(1);
1179 final double biasZ = ba.getElementAtIndex(2);
1180
1181 final KnownPositionAccelerometerCalibrator calibrator =
1182 new KnownPositionAccelerometerCalibrator(measurements,
1183 biasX, biasY, biasZ, this);
1184
1185
1186 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1187 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1188 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1189 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1190 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1191 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1192 final Acceleration bx2 = new Acceleration(0.0,
1193 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1194 calibrator.getInitialBiasXAsAcceleration(bx2);
1195 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1196 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1197 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1198 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1199 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1200 final Acceleration by2 = new Acceleration(0.0,
1201 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1202 calibrator.getInitialBiasYAsAcceleration(by2);
1203 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1204 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1205 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1206 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1207 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1208 final Acceleration bz2 = new Acceleration(0.0,
1209 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1210 calibrator.getInitialBiasZAsAcceleration(bz2);
1211 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1212 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1213 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1214 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1215 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1216 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1217 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1218 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1219 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1220 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1221 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1222 final double[] bias1 = calibrator.getInitialBias();
1223 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1224 final double[] bias2 = new double[3];
1225 calibrator.getInitialBias(bias2);
1226 assertArrayEquals(bias1, bias2, 0.0);
1227 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1228 assertEquals(b1, ba);
1229 final Matrix b2 = new Matrix(3, 1);
1230 calibrator.getInitialBiasAsMatrix(b2);
1231 assertEquals(b1, b2);
1232 final Matrix ma1 = calibrator.getInitialMa();
1233 assertEquals(ma1, new Matrix(3, 3));
1234 final Matrix ma2 = new Matrix(3, 3);
1235 calibrator.getInitialMa(ma2);
1236 assertEquals(ma1, ma2);
1237 assertNull(calibrator.getEcefPosition());
1238 assertNull(calibrator.getNedPosition());
1239 assertFalse(calibrator.getNedPosition(null));
1240 assertSame(calibrator.getMeasurements(), measurements);
1241 assertFalse(calibrator.isCommonAxisUsed());
1242 assertSame(calibrator.getListener(), this);
1243 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1244 assertFalse(calibrator.isReady());
1245 assertFalse(calibrator.isRunning());
1246 assertNull(calibrator.getEstimatedBiases());
1247 assertFalse(calibrator.getEstimatedBiases(null));
1248 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1249 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1250 assertNull(calibrator.getEstimatedBiasFx());
1251 assertNull(calibrator.getEstimatedBiasFy());
1252 assertNull(calibrator.getEstimatedBiasFz());
1253 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1254 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1255 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1256 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1257 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1258 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1259 assertNull(calibrator.getEstimatedMa());
1260 assertNull(calibrator.getEstimatedSx());
1261 assertNull(calibrator.getEstimatedSy());
1262 assertNull(calibrator.getEstimatedSz());
1263 assertNull(calibrator.getEstimatedMxy());
1264 assertNull(calibrator.getEstimatedMxz());
1265 assertNull(calibrator.getEstimatedMyx());
1266 assertNull(calibrator.getEstimatedMyz());
1267 assertNull(calibrator.getEstimatedMzx());
1268 assertNull(calibrator.getEstimatedMzy());
1269 assertNull(calibrator.getEstimatedCovariance());
1270 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1271 assertNull(calibrator.getGroundTruthGravityNorm());
1272 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1273 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1274 }
1275
1276 @Test
1277 public void testConstructor13() throws WrongSizeException {
1278 final Matrix ba = generateBa();
1279 final double biasX = ba.getElementAtIndex(0);
1280 final double biasY = ba.getElementAtIndex(1);
1281 final double biasZ = ba.getElementAtIndex(2);
1282
1283 final KnownPositionAccelerometerCalibrator calibrator =
1284 new KnownPositionAccelerometerCalibrator(true,
1285 biasX, biasY, biasZ);
1286
1287
1288 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1289 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1290 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1291 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1292 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1293 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1294 final Acceleration bx2 = new Acceleration(0.0,
1295 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1296 calibrator.getInitialBiasXAsAcceleration(bx2);
1297 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1298 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1299 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1300 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1301 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1302 final Acceleration by2 = new Acceleration(0.0,
1303 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1304 calibrator.getInitialBiasYAsAcceleration(by2);
1305 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1306 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1307 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1308 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1309 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1310 final Acceleration bz2 = new Acceleration(0.0,
1311 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1312 calibrator.getInitialBiasZAsAcceleration(bz2);
1313 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1314 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1315 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1316 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1317 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1318 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1319 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1320 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1321 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1322 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1323 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1324 final double[] bias1 = calibrator.getInitialBias();
1325 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1326 final double[] bias2 = new double[3];
1327 calibrator.getInitialBias(bias2);
1328 assertArrayEquals(bias1, bias2, 0.0);
1329 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1330 assertEquals(b1, ba);
1331 final Matrix b2 = new Matrix(3, 1);
1332 calibrator.getInitialBiasAsMatrix(b2);
1333 assertEquals(b1, b2);
1334 final Matrix ma1 = calibrator.getInitialMa();
1335 assertEquals(ma1, new Matrix(3, 3));
1336 final Matrix ma2 = new Matrix(3, 3);
1337 calibrator.getInitialMa(ma2);
1338 assertEquals(ma1, ma2);
1339 assertNull(calibrator.getEcefPosition());
1340 assertNull(calibrator.getNedPosition());
1341 assertFalse(calibrator.getNedPosition(null));
1342 assertNull(calibrator.getMeasurements());
1343 assertTrue(calibrator.isCommonAxisUsed());
1344 assertNull(calibrator.getListener());
1345 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1346 assertFalse(calibrator.isReady());
1347 assertFalse(calibrator.isRunning());
1348 assertNull(calibrator.getEstimatedBiases());
1349 assertFalse(calibrator.getEstimatedBiases(null));
1350 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1351 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1352 assertNull(calibrator.getEstimatedBiasFx());
1353 assertNull(calibrator.getEstimatedBiasFy());
1354 assertNull(calibrator.getEstimatedBiasFz());
1355 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1356 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1357 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1358 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1359 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1360 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1361 assertNull(calibrator.getEstimatedMa());
1362 assertNull(calibrator.getEstimatedSx());
1363 assertNull(calibrator.getEstimatedSy());
1364 assertNull(calibrator.getEstimatedSz());
1365 assertNull(calibrator.getEstimatedMxy());
1366 assertNull(calibrator.getEstimatedMxz());
1367 assertNull(calibrator.getEstimatedMyx());
1368 assertNull(calibrator.getEstimatedMyz());
1369 assertNull(calibrator.getEstimatedMzx());
1370 assertNull(calibrator.getEstimatedMzy());
1371 assertNull(calibrator.getEstimatedCovariance());
1372 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1373 assertNull(calibrator.getGroundTruthGravityNorm());
1374 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1375 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1376 }
1377
1378 @Test
1379 public void testConstructor14() throws WrongSizeException {
1380 final Matrix ba = generateBa();
1381 final double biasX = ba.getElementAtIndex(0);
1382 final double biasY = ba.getElementAtIndex(1);
1383 final double biasZ = ba.getElementAtIndex(2);
1384
1385 final KnownPositionAccelerometerCalibrator calibrator =
1386 new KnownPositionAccelerometerCalibrator(true,
1387 biasX, biasY, biasZ, this);
1388
1389
1390 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1391 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1392 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1393 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1394 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1395 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1396 final Acceleration bx2 = new Acceleration(0.0,
1397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1398 calibrator.getInitialBiasXAsAcceleration(bx2);
1399 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1400 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1401 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1402 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1403 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1404 final Acceleration by2 = new Acceleration(0.0,
1405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1406 calibrator.getInitialBiasYAsAcceleration(by2);
1407 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1408 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1409 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1410 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1411 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1412 final Acceleration bz2 = new Acceleration(0.0,
1413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1414 calibrator.getInitialBiasZAsAcceleration(bz2);
1415 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1416 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1417 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1418 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1419 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1420 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1421 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1422 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1423 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1424 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1425 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1426 final double[] bias1 = calibrator.getInitialBias();
1427 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1428 final double[] bias2 = new double[3];
1429 calibrator.getInitialBias(bias2);
1430 assertArrayEquals(bias1, bias2, 0.0);
1431 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1432 assertEquals(b1, ba);
1433 final Matrix b2 = new Matrix(3, 1);
1434 calibrator.getInitialBiasAsMatrix(b2);
1435 assertEquals(b1, b2);
1436 final Matrix ma1 = calibrator.getInitialMa();
1437 assertEquals(ma1, new Matrix(3, 3));
1438 final Matrix ma2 = new Matrix(3, 3);
1439 calibrator.getInitialMa(ma2);
1440 assertEquals(ma1, ma2);
1441 assertNull(calibrator.getEcefPosition());
1442 assertNull(calibrator.getNedPosition());
1443 assertFalse(calibrator.getNedPosition(null));
1444 assertNull(calibrator.getMeasurements());
1445 assertTrue(calibrator.isCommonAxisUsed());
1446 assertSame(calibrator.getListener(), this);
1447 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1448 assertFalse(calibrator.isReady());
1449 assertFalse(calibrator.isRunning());
1450 assertNull(calibrator.getEstimatedBiases());
1451 assertFalse(calibrator.getEstimatedBiases(null));
1452 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1453 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1454 assertNull(calibrator.getEstimatedBiasFx());
1455 assertNull(calibrator.getEstimatedBiasFy());
1456 assertNull(calibrator.getEstimatedBiasFz());
1457 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1458 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1459 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1460 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1461 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1462 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1463 assertNull(calibrator.getEstimatedMa());
1464 assertNull(calibrator.getEstimatedSx());
1465 assertNull(calibrator.getEstimatedSy());
1466 assertNull(calibrator.getEstimatedSz());
1467 assertNull(calibrator.getEstimatedMxy());
1468 assertNull(calibrator.getEstimatedMxz());
1469 assertNull(calibrator.getEstimatedMyx());
1470 assertNull(calibrator.getEstimatedMyz());
1471 assertNull(calibrator.getEstimatedMzx());
1472 assertNull(calibrator.getEstimatedMzy());
1473 assertNull(calibrator.getEstimatedCovariance());
1474 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1475 assertNull(calibrator.getGroundTruthGravityNorm());
1476 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1477 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1478 }
1479
1480 @Test
1481 public void testConstructor15() throws WrongSizeException {
1482 final Collection<StandardDeviationBodyKinematics> measurements =
1483 Collections.emptyList();
1484
1485 final Matrix ba = generateBa();
1486 final double biasX = ba.getElementAtIndex(0);
1487 final double biasY = ba.getElementAtIndex(1);
1488 final double biasZ = ba.getElementAtIndex(2);
1489
1490 final KnownPositionAccelerometerCalibrator calibrator =
1491 new KnownPositionAccelerometerCalibrator(measurements,
1492 true, biasX, biasY, biasZ);
1493
1494
1495 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1496 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1497 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1498 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1499 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1500 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1501 final Acceleration bx2 = new Acceleration(0.0,
1502 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1503 calibrator.getInitialBiasXAsAcceleration(bx2);
1504 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1505 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1506 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1507 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1508 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1509 final Acceleration by2 = new Acceleration(0.0,
1510 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1511 calibrator.getInitialBiasYAsAcceleration(by2);
1512 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1513 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1514 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1515 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1516 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1517 final Acceleration bz2 = new Acceleration(0.0,
1518 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1519 calibrator.getInitialBiasZAsAcceleration(bz2);
1520 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1521 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1522 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1523 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1524 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1525 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1526 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1527 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1528 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1529 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1530 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1531 final double[] bias1 = calibrator.getInitialBias();
1532 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1533 final double[] bias2 = new double[3];
1534 calibrator.getInitialBias(bias2);
1535 assertArrayEquals(bias1, bias2, 0.0);
1536 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1537 assertEquals(b1, ba);
1538 final Matrix b2 = new Matrix(3, 1);
1539 calibrator.getInitialBiasAsMatrix(b2);
1540 assertEquals(b1, b2);
1541 final Matrix ma1 = calibrator.getInitialMa();
1542 assertEquals(ma1, new Matrix(3, 3));
1543 final Matrix ma2 = new Matrix(3, 3);
1544 calibrator.getInitialMa(ma2);
1545 assertEquals(ma1, ma2);
1546 assertNull(calibrator.getEcefPosition());
1547 assertNull(calibrator.getNedPosition());
1548 assertFalse(calibrator.getNedPosition(null));
1549 assertSame(calibrator.getMeasurements(), measurements);
1550 assertTrue(calibrator.isCommonAxisUsed());
1551 assertNull(calibrator.getListener());
1552 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1553 assertFalse(calibrator.isReady());
1554 assertFalse(calibrator.isRunning());
1555 assertNull(calibrator.getEstimatedBiases());
1556 assertFalse(calibrator.getEstimatedBiases(null));
1557 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1558 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1559 assertNull(calibrator.getEstimatedBiasFx());
1560 assertNull(calibrator.getEstimatedBiasFy());
1561 assertNull(calibrator.getEstimatedBiasFz());
1562 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1563 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1564 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1565 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1566 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1567 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1568 assertNull(calibrator.getEstimatedMa());
1569 assertNull(calibrator.getEstimatedSx());
1570 assertNull(calibrator.getEstimatedSy());
1571 assertNull(calibrator.getEstimatedSz());
1572 assertNull(calibrator.getEstimatedMxy());
1573 assertNull(calibrator.getEstimatedMxz());
1574 assertNull(calibrator.getEstimatedMyx());
1575 assertNull(calibrator.getEstimatedMyz());
1576 assertNull(calibrator.getEstimatedMzx());
1577 assertNull(calibrator.getEstimatedMzy());
1578 assertNull(calibrator.getEstimatedCovariance());
1579 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1580 assertNull(calibrator.getGroundTruthGravityNorm());
1581 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1582 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1583 }
1584
1585 @Test
1586 public void testConstructor16() throws WrongSizeException {
1587 final Collection<StandardDeviationBodyKinematics> measurements =
1588 Collections.emptyList();
1589
1590 final Matrix ba = generateBa();
1591 final double biasX = ba.getElementAtIndex(0);
1592 final double biasY = ba.getElementAtIndex(1);
1593 final double biasZ = ba.getElementAtIndex(2);
1594
1595 final KnownPositionAccelerometerCalibrator calibrator =
1596 new KnownPositionAccelerometerCalibrator(measurements,
1597 true, biasX, biasY, biasZ, this);
1598
1599
1600 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1601 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1602 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1603 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1604 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1605 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1606 final Acceleration bx2 = new Acceleration(0.0,
1607 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1608 calibrator.getInitialBiasXAsAcceleration(bx2);
1609 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1610 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1611 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1612 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1613 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1614 final Acceleration by2 = new Acceleration(0.0,
1615 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1616 calibrator.getInitialBiasYAsAcceleration(by2);
1617 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1618 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1619 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1620 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1621 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1622 final Acceleration bz2 = new Acceleration(0.0,
1623 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1624 calibrator.getInitialBiasZAsAcceleration(bz2);
1625 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1626 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1627 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1628 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1629 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1630 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1631 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1632 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1633 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1634 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1635 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1636 final double[] bias1 = calibrator.getInitialBias();
1637 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1638 final double[] bias2 = new double[3];
1639 calibrator.getInitialBias(bias2);
1640 assertArrayEquals(bias1, bias2, 0.0);
1641 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1642 assertEquals(b1, ba);
1643 final Matrix b2 = new Matrix(3, 1);
1644 calibrator.getInitialBiasAsMatrix(b2);
1645 assertEquals(b1, b2);
1646 final Matrix ma1 = calibrator.getInitialMa();
1647 assertEquals(ma1, new Matrix(3, 3));
1648 final Matrix ma2 = new Matrix(3, 3);
1649 calibrator.getInitialMa(ma2);
1650 assertEquals(ma1, ma2);
1651 assertNull(calibrator.getEcefPosition());
1652 assertNull(calibrator.getNedPosition());
1653 assertFalse(calibrator.getNedPosition(null));
1654 assertSame(calibrator.getMeasurements(), measurements);
1655 assertTrue(calibrator.isCommonAxisUsed());
1656 assertSame(calibrator.getListener(), this);
1657 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
1658 assertFalse(calibrator.isReady());
1659 assertFalse(calibrator.isRunning());
1660 assertNull(calibrator.getEstimatedBiases());
1661 assertFalse(calibrator.getEstimatedBiases(null));
1662 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1663 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1664 assertNull(calibrator.getEstimatedBiasFx());
1665 assertNull(calibrator.getEstimatedBiasFy());
1666 assertNull(calibrator.getEstimatedBiasFz());
1667 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1668 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1669 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1670 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1671 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1672 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1673 assertNull(calibrator.getEstimatedMa());
1674 assertNull(calibrator.getEstimatedSx());
1675 assertNull(calibrator.getEstimatedSy());
1676 assertNull(calibrator.getEstimatedSz());
1677 assertNull(calibrator.getEstimatedMxy());
1678 assertNull(calibrator.getEstimatedMxz());
1679 assertNull(calibrator.getEstimatedMyx());
1680 assertNull(calibrator.getEstimatedMyz());
1681 assertNull(calibrator.getEstimatedMzx());
1682 assertNull(calibrator.getEstimatedMzy());
1683 assertNull(calibrator.getEstimatedCovariance());
1684 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1685 assertNull(calibrator.getGroundTruthGravityNorm());
1686 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1687 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1688 }
1689
1690 @Test
1691 public void testConstructor17() throws WrongSizeException {
1692 final Matrix ba = generateBa();
1693 final double biasX = ba.getElementAtIndex(0);
1694 final double biasY = ba.getElementAtIndex(1);
1695 final double biasZ = ba.getElementAtIndex(2);
1696
1697 final Acceleration bx = new Acceleration(biasX,
1698 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1699 final Acceleration by = new Acceleration(biasY,
1700 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1701 final Acceleration bz = new Acceleration(biasZ,
1702 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1703
1704 final KnownPositionAccelerometerCalibrator calibrator =
1705 new KnownPositionAccelerometerCalibrator(bx, by, bz);
1706
1707
1708 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1709 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1710 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1711 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1712 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1713 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1714 final Acceleration bx2 = new Acceleration(0.0,
1715 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1716 calibrator.getInitialBiasXAsAcceleration(bx2);
1717 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1718 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1719 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1720 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1721 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1722 final Acceleration by2 = new Acceleration(0.0,
1723 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1724 calibrator.getInitialBiasYAsAcceleration(by2);
1725 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1726 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1727 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1728 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1729 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1730 final Acceleration bz2 = new Acceleration(0.0,
1731 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1732 calibrator.getInitialBiasZAsAcceleration(bz2);
1733 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1734 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1735 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1736 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1737 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1738 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1739 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1740 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1741 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1742 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1743 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1744 final double[] bias1 = calibrator.getInitialBias();
1745 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1746 final double[] bias2 = new double[3];
1747 calibrator.getInitialBias(bias2);
1748 assertArrayEquals(bias1, bias2, 0.0);
1749 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1750 assertEquals(b1, ba);
1751 final Matrix b2 = new Matrix(3, 1);
1752 calibrator.getInitialBiasAsMatrix(b2);
1753 assertEquals(b1, b2);
1754 final Matrix ma1 = calibrator.getInitialMa();
1755 assertEquals(ma1, new Matrix(3, 3));
1756 final Matrix ma2 = new Matrix(3, 3);
1757 calibrator.getInitialMa(ma2);
1758 assertEquals(ma1, ma2);
1759 assertNull(calibrator.getEcefPosition());
1760 assertNull(calibrator.getNedPosition());
1761 assertFalse(calibrator.getNedPosition(null));
1762 assertNull(calibrator.getMeasurements());
1763 assertFalse(calibrator.isCommonAxisUsed());
1764 assertNull(calibrator.getListener());
1765 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1766 assertFalse(calibrator.isReady());
1767 assertFalse(calibrator.isRunning());
1768 assertNull(calibrator.getEstimatedBiases());
1769 assertFalse(calibrator.getEstimatedBiases(null));
1770 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1771 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1772 assertNull(calibrator.getEstimatedBiasFx());
1773 assertNull(calibrator.getEstimatedBiasFy());
1774 assertNull(calibrator.getEstimatedBiasFz());
1775 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1776 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1777 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1778 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1779 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1780 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1781 assertNull(calibrator.getEstimatedMa());
1782 assertNull(calibrator.getEstimatedSx());
1783 assertNull(calibrator.getEstimatedSy());
1784 assertNull(calibrator.getEstimatedSz());
1785 assertNull(calibrator.getEstimatedMxy());
1786 assertNull(calibrator.getEstimatedMxz());
1787 assertNull(calibrator.getEstimatedMyx());
1788 assertNull(calibrator.getEstimatedMyz());
1789 assertNull(calibrator.getEstimatedMzx());
1790 assertNull(calibrator.getEstimatedMzy());
1791 assertNull(calibrator.getEstimatedCovariance());
1792 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1793 assertNull(calibrator.getGroundTruthGravityNorm());
1794 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1795 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1796 }
1797
1798 @Test
1799 public void testConstructor18() throws WrongSizeException {
1800 final Matrix ba = generateBa();
1801 final double biasX = ba.getElementAtIndex(0);
1802 final double biasY = ba.getElementAtIndex(1);
1803 final double biasZ = ba.getElementAtIndex(2);
1804
1805 final Acceleration bx = new Acceleration(biasX,
1806 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1807 final Acceleration by = new Acceleration(biasY,
1808 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1809 final Acceleration bz = new Acceleration(biasZ,
1810 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1811
1812 final KnownPositionAccelerometerCalibrator calibrator =
1813 new KnownPositionAccelerometerCalibrator(bx, by, bz,
1814 this);
1815
1816
1817 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1818 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1819 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1820 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1821 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1822 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1823 final Acceleration bx2 = new Acceleration(0.0,
1824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1825 calibrator.getInitialBiasXAsAcceleration(bx2);
1826 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1827 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1828 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1829 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1830 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1831 final Acceleration by2 = new Acceleration(0.0,
1832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1833 calibrator.getInitialBiasYAsAcceleration(by2);
1834 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1835 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1836 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1837 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1838 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1839 final Acceleration bz2 = new Acceleration(0.0,
1840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1841 calibrator.getInitialBiasZAsAcceleration(bz2);
1842 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1843 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1844 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1845 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1846 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1847 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1848 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1849 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1850 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1851 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1852 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1853 final double[] bias1 = calibrator.getInitialBias();
1854 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1855 final double[] bias2 = new double[3];
1856 calibrator.getInitialBias(bias2);
1857 assertArrayEquals(bias1, bias2, 0.0);
1858 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1859 assertEquals(b1, ba);
1860 final Matrix b2 = new Matrix(3, 1);
1861 calibrator.getInitialBiasAsMatrix(b2);
1862 assertEquals(b1, b2);
1863 final Matrix ma1 = calibrator.getInitialMa();
1864 assertEquals(ma1, new Matrix(3, 3));
1865 final Matrix ma2 = new Matrix(3, 3);
1866 calibrator.getInitialMa(ma2);
1867 assertEquals(ma1, ma2);
1868 assertNull(calibrator.getEcefPosition());
1869 assertNull(calibrator.getNedPosition());
1870 assertFalse(calibrator.getNedPosition(null));
1871 assertNull(calibrator.getMeasurements());
1872 assertFalse(calibrator.isCommonAxisUsed());
1873 assertSame(calibrator.getListener(), this);
1874 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1875 assertFalse(calibrator.isReady());
1876 assertFalse(calibrator.isRunning());
1877 assertNull(calibrator.getEstimatedBiases());
1878 assertFalse(calibrator.getEstimatedBiases(null));
1879 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1880 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1881 assertNull(calibrator.getEstimatedBiasFx());
1882 assertNull(calibrator.getEstimatedBiasFy());
1883 assertNull(calibrator.getEstimatedBiasFz());
1884 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1885 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1886 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1887 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
1888 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
1889 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
1890 assertNull(calibrator.getEstimatedMa());
1891 assertNull(calibrator.getEstimatedSx());
1892 assertNull(calibrator.getEstimatedSy());
1893 assertNull(calibrator.getEstimatedSz());
1894 assertNull(calibrator.getEstimatedMxy());
1895 assertNull(calibrator.getEstimatedMxz());
1896 assertNull(calibrator.getEstimatedMyx());
1897 assertNull(calibrator.getEstimatedMyz());
1898 assertNull(calibrator.getEstimatedMzx());
1899 assertNull(calibrator.getEstimatedMzy());
1900 assertNull(calibrator.getEstimatedCovariance());
1901 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
1902 assertNull(calibrator.getGroundTruthGravityNorm());
1903 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
1904 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
1905 }
1906
1907 @Test
1908 public void testConstructor19() throws WrongSizeException {
1909 final Collection<StandardDeviationBodyKinematics> measurements =
1910 Collections.emptyList();
1911
1912 final Matrix ba = generateBa();
1913 final double biasX = ba.getElementAtIndex(0);
1914 final double biasY = ba.getElementAtIndex(1);
1915 final double biasZ = ba.getElementAtIndex(2);
1916
1917 final Acceleration bx = new Acceleration(biasX,
1918 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1919 final Acceleration by = new Acceleration(biasY,
1920 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1921 final Acceleration bz = new Acceleration(biasZ,
1922 AccelerationUnit.METERS_PER_SQUARED_SECOND);
1923
1924 final KnownPositionAccelerometerCalibrator calibrator =
1925 new KnownPositionAccelerometerCalibrator(measurements,
1926 bx, by, bz);
1927
1928
1929 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
1930 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
1931 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
1932 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
1933 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
1934 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1935 final Acceleration bx2 = new Acceleration(0.0,
1936 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1937 calibrator.getInitialBiasXAsAcceleration(bx2);
1938 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
1939 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1940 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
1941 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
1942 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1943 final Acceleration by2 = new Acceleration(0.0,
1944 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1945 calibrator.getInitialBiasYAsAcceleration(by2);
1946 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
1947 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1948 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
1949 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
1950 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1951 final Acceleration bz2 = new Acceleration(0.0,
1952 AccelerationUnit.FEET_PER_SQUARED_SECOND);
1953 calibrator.getInitialBiasZAsAcceleration(bz2);
1954 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
1955 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
1956 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
1957 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
1958 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
1959 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
1960 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
1961 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
1962 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
1963 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
1964 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
1965 final double[] bias1 = calibrator.getInitialBias();
1966 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
1967 final double[] bias2 = new double[3];
1968 calibrator.getInitialBias(bias2);
1969 assertArrayEquals(bias1, bias2, 0.0);
1970 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
1971 assertEquals(b1, ba);
1972 final Matrix b2 = new Matrix(3, 1);
1973 calibrator.getInitialBiasAsMatrix(b2);
1974 assertEquals(b1, b2);
1975 final Matrix ma1 = calibrator.getInitialMa();
1976 assertEquals(ma1, new Matrix(3, 3));
1977 final Matrix ma2 = new Matrix(3, 3);
1978 calibrator.getInitialMa(ma2);
1979 assertEquals(ma1, ma2);
1980 assertNull(calibrator.getEcefPosition());
1981 assertNull(calibrator.getNedPosition());
1982 assertFalse(calibrator.getNedPosition(null));
1983 assertSame(calibrator.getMeasurements(), measurements);
1984 assertFalse(calibrator.isCommonAxisUsed());
1985 assertNull(calibrator.getListener());
1986 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
1987 assertFalse(calibrator.isReady());
1988 assertFalse(calibrator.isRunning());
1989 assertNull(calibrator.getEstimatedBiases());
1990 assertFalse(calibrator.getEstimatedBiases(null));
1991 assertNull(calibrator.getEstimatedBiasesAsMatrix());
1992 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
1993 assertNull(calibrator.getEstimatedBiasFx());
1994 assertNull(calibrator.getEstimatedBiasFy());
1995 assertNull(calibrator.getEstimatedBiasFz());
1996 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
1997 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
1998 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
1999 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2000 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2001 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2002 assertNull(calibrator.getEstimatedMa());
2003 assertNull(calibrator.getEstimatedSx());
2004 assertNull(calibrator.getEstimatedSy());
2005 assertNull(calibrator.getEstimatedSz());
2006 assertNull(calibrator.getEstimatedMxy());
2007 assertNull(calibrator.getEstimatedMxz());
2008 assertNull(calibrator.getEstimatedMyx());
2009 assertNull(calibrator.getEstimatedMyz());
2010 assertNull(calibrator.getEstimatedMzx());
2011 assertNull(calibrator.getEstimatedMzy());
2012 assertNull(calibrator.getEstimatedCovariance());
2013 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2014 assertNull(calibrator.getGroundTruthGravityNorm());
2015 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2016 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2017 }
2018
2019 @Test
2020 public void testConstructor20() throws WrongSizeException {
2021 final Collection<StandardDeviationBodyKinematics> measurements =
2022 Collections.emptyList();
2023
2024 final Matrix ba = generateBa();
2025 final double biasX = ba.getElementAtIndex(0);
2026 final double biasY = ba.getElementAtIndex(1);
2027 final double biasZ = ba.getElementAtIndex(2);
2028
2029 final Acceleration bx = new Acceleration(biasX,
2030 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2031 final Acceleration by = new Acceleration(biasY,
2032 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2033 final Acceleration bz = new Acceleration(biasZ,
2034 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2035
2036 final KnownPositionAccelerometerCalibrator calibrator =
2037 new KnownPositionAccelerometerCalibrator(measurements,
2038 bx, by, bz, this);
2039
2040
2041 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2042 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2043 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2044 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2045 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2046 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2047 final Acceleration bx2 = new Acceleration(0.0,
2048 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2049 calibrator.getInitialBiasXAsAcceleration(bx2);
2050 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2051 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2052 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2053 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2054 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2055 final Acceleration by2 = new Acceleration(0.0,
2056 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2057 calibrator.getInitialBiasYAsAcceleration(by2);
2058 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2059 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2060 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2061 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2062 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2063 final Acceleration bz2 = new Acceleration(0.0,
2064 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2065 calibrator.getInitialBiasZAsAcceleration(bz2);
2066 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2067 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2068 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2069 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2070 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2071 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2072 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2073 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2074 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2075 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2076 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2077 final double[] bias1 = calibrator.getInitialBias();
2078 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2079 final double[] bias2 = new double[3];
2080 calibrator.getInitialBias(bias2);
2081 assertArrayEquals(bias1, bias2, 0.0);
2082 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2083 assertEquals(b1, ba);
2084 final Matrix b2 = new Matrix(3, 1);
2085 calibrator.getInitialBiasAsMatrix(b2);
2086 assertEquals(b1, b2);
2087 final Matrix ma1 = calibrator.getInitialMa();
2088 assertEquals(ma1, new Matrix(3, 3));
2089 final Matrix ma2 = new Matrix(3, 3);
2090 calibrator.getInitialMa(ma2);
2091 assertEquals(ma1, ma2);
2092 assertNull(calibrator.getEcefPosition());
2093 assertNull(calibrator.getNedPosition());
2094 assertFalse(calibrator.getNedPosition(null));
2095 assertSame(calibrator.getMeasurements(), measurements);
2096 assertFalse(calibrator.isCommonAxisUsed());
2097 assertSame(calibrator.getListener(), this);
2098 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2099 assertFalse(calibrator.isReady());
2100 assertFalse(calibrator.isRunning());
2101 assertNull(calibrator.getEstimatedBiases());
2102 assertFalse(calibrator.getEstimatedBiases(null));
2103 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2104 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2105 assertNull(calibrator.getEstimatedBiasFx());
2106 assertNull(calibrator.getEstimatedBiasFy());
2107 assertNull(calibrator.getEstimatedBiasFz());
2108 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2109 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2110 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2111 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2112 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2113 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2114 assertNull(calibrator.getEstimatedMa());
2115 assertNull(calibrator.getEstimatedSx());
2116 assertNull(calibrator.getEstimatedSy());
2117 assertNull(calibrator.getEstimatedSz());
2118 assertNull(calibrator.getEstimatedMxy());
2119 assertNull(calibrator.getEstimatedMxz());
2120 assertNull(calibrator.getEstimatedMyx());
2121 assertNull(calibrator.getEstimatedMyz());
2122 assertNull(calibrator.getEstimatedMzx());
2123 assertNull(calibrator.getEstimatedMzy());
2124 assertNull(calibrator.getEstimatedCovariance());
2125 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2126 assertNull(calibrator.getGroundTruthGravityNorm());
2127 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2128 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2129 }
2130
2131 @Test
2132 public void testConstructor21() throws WrongSizeException {
2133 final Matrix ba = generateBa();
2134 final double biasX = ba.getElementAtIndex(0);
2135 final double biasY = ba.getElementAtIndex(1);
2136 final double biasZ = ba.getElementAtIndex(2);
2137
2138 final Acceleration bx = new Acceleration(biasX,
2139 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2140 final Acceleration by = new Acceleration(biasY,
2141 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2142 final Acceleration bz = new Acceleration(biasZ,
2143 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2144
2145 final KnownPositionAccelerometerCalibrator calibrator =
2146 new KnownPositionAccelerometerCalibrator(true,
2147 bx, by, bz);
2148
2149
2150 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2151 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2152 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2153 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2154 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2155 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2156 final Acceleration bx2 = new Acceleration(0.0,
2157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2158 calibrator.getInitialBiasXAsAcceleration(bx2);
2159 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2160 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2161 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2162 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2163 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2164 final Acceleration by2 = new Acceleration(0.0,
2165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2166 calibrator.getInitialBiasYAsAcceleration(by2);
2167 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2168 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2169 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2170 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2171 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2172 final Acceleration bz2 = new Acceleration(0.0,
2173 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2174 calibrator.getInitialBiasZAsAcceleration(bz2);
2175 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2176 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2177 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2178 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2179 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2180 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2181 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2182 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2183 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2184 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2185 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2186 final double[] bias1 = calibrator.getInitialBias();
2187 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2188 final double[] bias2 = new double[3];
2189 calibrator.getInitialBias(bias2);
2190 assertArrayEquals(bias1, bias2, 0.0);
2191 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2192 assertEquals(b1, ba);
2193 final Matrix b2 = new Matrix(3, 1);
2194 calibrator.getInitialBiasAsMatrix(b2);
2195 assertEquals(b1, b2);
2196 final Matrix ma1 = calibrator.getInitialMa();
2197 assertEquals(ma1, new Matrix(3, 3));
2198 final Matrix ma2 = new Matrix(3, 3);
2199 calibrator.getInitialMa(ma2);
2200 assertEquals(ma1, ma2);
2201 assertNull(calibrator.getEcefPosition());
2202 assertNull(calibrator.getNedPosition());
2203 assertFalse(calibrator.getNedPosition(null));
2204 assertNull(calibrator.getMeasurements());
2205 assertTrue(calibrator.isCommonAxisUsed());
2206 assertNull(calibrator.getListener());
2207 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2208 assertFalse(calibrator.isReady());
2209 assertFalse(calibrator.isRunning());
2210 assertNull(calibrator.getEstimatedBiases());
2211 assertFalse(calibrator.getEstimatedBiases(null));
2212 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2213 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2214 assertNull(calibrator.getEstimatedBiasFx());
2215 assertNull(calibrator.getEstimatedBiasFy());
2216 assertNull(calibrator.getEstimatedBiasFz());
2217 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2218 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2219 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2220 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2221 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2222 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2223 assertNull(calibrator.getEstimatedMa());
2224 assertNull(calibrator.getEstimatedSx());
2225 assertNull(calibrator.getEstimatedSy());
2226 assertNull(calibrator.getEstimatedSz());
2227 assertNull(calibrator.getEstimatedMxy());
2228 assertNull(calibrator.getEstimatedMxz());
2229 assertNull(calibrator.getEstimatedMyx());
2230 assertNull(calibrator.getEstimatedMyz());
2231 assertNull(calibrator.getEstimatedMzx());
2232 assertNull(calibrator.getEstimatedMzy());
2233 assertNull(calibrator.getEstimatedCovariance());
2234 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2235 assertNull(calibrator.getGroundTruthGravityNorm());
2236 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2237 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2238 }
2239
2240 @Test
2241 public void testConstructor22() throws WrongSizeException {
2242 final Matrix ba = generateBa();
2243 final double biasX = ba.getElementAtIndex(0);
2244 final double biasY = ba.getElementAtIndex(1);
2245 final double biasZ = ba.getElementAtIndex(2);
2246
2247 final Acceleration bx = new Acceleration(biasX,
2248 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2249 final Acceleration by = new Acceleration(biasY,
2250 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2251 final Acceleration bz = new Acceleration(biasZ,
2252 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2253
2254 final KnownPositionAccelerometerCalibrator calibrator =
2255 new KnownPositionAccelerometerCalibrator(true,
2256 bx, by, bz, this);
2257
2258
2259 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2260 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2261 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2262 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2263 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2264 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2265 final Acceleration bx2 = new Acceleration(0.0,
2266 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2267 calibrator.getInitialBiasXAsAcceleration(bx2);
2268 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2269 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2270 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2271 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2272 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2273 final Acceleration by2 = new Acceleration(0.0,
2274 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2275 calibrator.getInitialBiasYAsAcceleration(by2);
2276 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2277 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2278 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2279 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2280 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2281 final Acceleration bz2 = new Acceleration(0.0,
2282 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2283 calibrator.getInitialBiasZAsAcceleration(bz2);
2284 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2285 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2286 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2287 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2288 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2289 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2290 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2291 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2292 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2293 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2294 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2295 final double[] bias1 = calibrator.getInitialBias();
2296 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2297 final double[] bias2 = new double[3];
2298 calibrator.getInitialBias(bias2);
2299 assertArrayEquals(bias1, bias2, 0.0);
2300 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2301 assertEquals(b1, ba);
2302 final Matrix b2 = new Matrix(3, 1);
2303 calibrator.getInitialBiasAsMatrix(b2);
2304 assertEquals(b1, b2);
2305 final Matrix ma1 = calibrator.getInitialMa();
2306 assertEquals(ma1, new Matrix(3, 3));
2307 final Matrix ma2 = new Matrix(3, 3);
2308 calibrator.getInitialMa(ma2);
2309 assertEquals(ma1, ma2);
2310 assertNull(calibrator.getEcefPosition());
2311 assertNull(calibrator.getNedPosition());
2312 assertFalse(calibrator.getNedPosition(null));
2313 assertNull(calibrator.getMeasurements());
2314 assertTrue(calibrator.isCommonAxisUsed());
2315 assertSame(calibrator.getListener(), this);
2316 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2317 assertFalse(calibrator.isReady());
2318 assertFalse(calibrator.isRunning());
2319 assertNull(calibrator.getEstimatedBiases());
2320 assertFalse(calibrator.getEstimatedBiases(null));
2321 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2322 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2323 assertNull(calibrator.getEstimatedBiasFx());
2324 assertNull(calibrator.getEstimatedBiasFy());
2325 assertNull(calibrator.getEstimatedBiasFz());
2326 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2327 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2328 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2329 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2330 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2331 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2332 assertNull(calibrator.getEstimatedMa());
2333 assertNull(calibrator.getEstimatedSx());
2334 assertNull(calibrator.getEstimatedSy());
2335 assertNull(calibrator.getEstimatedSz());
2336 assertNull(calibrator.getEstimatedMxy());
2337 assertNull(calibrator.getEstimatedMxz());
2338 assertNull(calibrator.getEstimatedMyx());
2339 assertNull(calibrator.getEstimatedMyz());
2340 assertNull(calibrator.getEstimatedMzx());
2341 assertNull(calibrator.getEstimatedMzy());
2342 assertNull(calibrator.getEstimatedCovariance());
2343 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2344 assertNull(calibrator.getGroundTruthGravityNorm());
2345 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2346 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2347 }
2348
2349 @Test
2350 public void testConstructor23() throws WrongSizeException {
2351 final Collection<StandardDeviationBodyKinematics> measurements =
2352 Collections.emptyList();
2353
2354 final Matrix ba = generateBa();
2355 final double biasX = ba.getElementAtIndex(0);
2356 final double biasY = ba.getElementAtIndex(1);
2357 final double biasZ = ba.getElementAtIndex(2);
2358
2359 final Acceleration bx = new Acceleration(biasX,
2360 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2361 final Acceleration by = new Acceleration(biasY,
2362 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2363 final Acceleration bz = new Acceleration(biasZ,
2364 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2365
2366 final KnownPositionAccelerometerCalibrator calibrator =
2367 new KnownPositionAccelerometerCalibrator(measurements,
2368 true, bx, by, bz);
2369
2370
2371 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2372 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2373 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2374 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2375 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2376 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2377 final Acceleration bx2 = new Acceleration(0.0,
2378 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2379 calibrator.getInitialBiasXAsAcceleration(bx2);
2380 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2381 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2382 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2383 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2384 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2385 final Acceleration by2 = new Acceleration(0.0,
2386 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2387 calibrator.getInitialBiasYAsAcceleration(by2);
2388 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2389 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2390 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2391 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2392 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2393 final Acceleration bz2 = new Acceleration(0.0,
2394 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2395 calibrator.getInitialBiasZAsAcceleration(bz2);
2396 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2397 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2398 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2399 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2400 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2401 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2402 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2403 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2404 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2405 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2406 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2407 final double[] bias1 = calibrator.getInitialBias();
2408 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2409 final double[] bias2 = new double[3];
2410 calibrator.getInitialBias(bias2);
2411 assertArrayEquals(bias1, bias2, 0.0);
2412 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2413 assertEquals(b1, ba);
2414 final Matrix b2 = new Matrix(3, 1);
2415 calibrator.getInitialBiasAsMatrix(b2);
2416 assertEquals(b1, b2);
2417 final Matrix ma1 = calibrator.getInitialMa();
2418 assertEquals(ma1, new Matrix(3, 3));
2419 final Matrix ma2 = new Matrix(3, 3);
2420 calibrator.getInitialMa(ma2);
2421 assertEquals(ma1, ma2);
2422 assertNull(calibrator.getEcefPosition());
2423 assertNull(calibrator.getNedPosition());
2424 assertFalse(calibrator.getNedPosition(null));
2425 assertSame(calibrator.getMeasurements(), measurements);
2426 assertTrue(calibrator.isCommonAxisUsed());
2427 assertNull(calibrator.getListener());
2428 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2429 assertFalse(calibrator.isReady());
2430 assertFalse(calibrator.isRunning());
2431 assertNull(calibrator.getEstimatedBiases());
2432 assertFalse(calibrator.getEstimatedBiases(null));
2433 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2434 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2435 assertNull(calibrator.getEstimatedBiasFx());
2436 assertNull(calibrator.getEstimatedBiasFy());
2437 assertNull(calibrator.getEstimatedBiasFz());
2438 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2439 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2440 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2441 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2442 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2443 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2444 assertNull(calibrator.getEstimatedMa());
2445 assertNull(calibrator.getEstimatedSx());
2446 assertNull(calibrator.getEstimatedSy());
2447 assertNull(calibrator.getEstimatedSz());
2448 assertNull(calibrator.getEstimatedMxy());
2449 assertNull(calibrator.getEstimatedMxz());
2450 assertNull(calibrator.getEstimatedMyx());
2451 assertNull(calibrator.getEstimatedMyz());
2452 assertNull(calibrator.getEstimatedMzx());
2453 assertNull(calibrator.getEstimatedMzy());
2454 assertNull(calibrator.getEstimatedCovariance());
2455 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2456 assertNull(calibrator.getGroundTruthGravityNorm());
2457 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2458 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2459 }
2460
2461 @Test
2462 public void testConstructor24() throws WrongSizeException {
2463 final Collection<StandardDeviationBodyKinematics> measurements =
2464 Collections.emptyList();
2465
2466 final Matrix ba = generateBa();
2467 final double biasX = ba.getElementAtIndex(0);
2468 final double biasY = ba.getElementAtIndex(1);
2469 final double biasZ = ba.getElementAtIndex(2);
2470
2471 final Acceleration bx = new Acceleration(biasX,
2472 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2473 final Acceleration by = new Acceleration(biasY,
2474 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2475 final Acceleration bz = new Acceleration(biasZ,
2476 AccelerationUnit.METERS_PER_SQUARED_SECOND);
2477
2478 final KnownPositionAccelerometerCalibrator calibrator =
2479 new KnownPositionAccelerometerCalibrator(measurements,
2480 true, bx, by, bz, this);
2481
2482
2483 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2484 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2485 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2486 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2487 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2488 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2489 final Acceleration bx2 = new Acceleration(0.0,
2490 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2491 calibrator.getInitialBiasXAsAcceleration(bx2);
2492 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2493 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2494 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2495 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2496 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2497 final Acceleration by2 = new Acceleration(0.0,
2498 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2499 calibrator.getInitialBiasYAsAcceleration(by2);
2500 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2501 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2502 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2503 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2504 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2505 final Acceleration bz2 = new Acceleration(0.0,
2506 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2507 calibrator.getInitialBiasZAsAcceleration(bz2);
2508 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2509 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2510 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
2511 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
2512 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
2513 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2514 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2515 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2516 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2517 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2518 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2519 final double[] bias1 = calibrator.getInitialBias();
2520 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2521 final double[] bias2 = new double[3];
2522 calibrator.getInitialBias(bias2);
2523 assertArrayEquals(bias1, bias2, 0.0);
2524 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2525 assertEquals(b1, ba);
2526 final Matrix b2 = new Matrix(3, 1);
2527 calibrator.getInitialBiasAsMatrix(b2);
2528 assertEquals(b1, b2);
2529 final Matrix ma1 = calibrator.getInitialMa();
2530 assertEquals(ma1, new Matrix(3, 3));
2531 final Matrix ma2 = new Matrix(3, 3);
2532 calibrator.getInitialMa(ma2);
2533 assertEquals(ma1, ma2);
2534 assertNull(calibrator.getEcefPosition());
2535 assertNull(calibrator.getNedPosition());
2536 assertFalse(calibrator.getNedPosition(null));
2537 assertSame(calibrator.getMeasurements(), measurements);
2538 assertTrue(calibrator.isCommonAxisUsed());
2539 assertSame(calibrator.getListener(), this);
2540 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2541 assertFalse(calibrator.isReady());
2542 assertFalse(calibrator.isRunning());
2543 assertNull(calibrator.getEstimatedBiases());
2544 assertFalse(calibrator.getEstimatedBiases(null));
2545 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2546 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2547 assertNull(calibrator.getEstimatedBiasFx());
2548 assertNull(calibrator.getEstimatedBiasFy());
2549 assertNull(calibrator.getEstimatedBiasFz());
2550 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2551 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2552 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2553 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2554 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2555 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2556 assertNull(calibrator.getEstimatedMa());
2557 assertNull(calibrator.getEstimatedSx());
2558 assertNull(calibrator.getEstimatedSy());
2559 assertNull(calibrator.getEstimatedSz());
2560 assertNull(calibrator.getEstimatedMxy());
2561 assertNull(calibrator.getEstimatedMxz());
2562 assertNull(calibrator.getEstimatedMyx());
2563 assertNull(calibrator.getEstimatedMyz());
2564 assertNull(calibrator.getEstimatedMzx());
2565 assertNull(calibrator.getEstimatedMzy());
2566 assertNull(calibrator.getEstimatedCovariance());
2567 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2568 assertNull(calibrator.getGroundTruthGravityNorm());
2569 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2570 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2571 }
2572
2573 @Test
2574 public void testConstructor25() throws WrongSizeException {
2575 final Matrix ba = generateBa();
2576 final double biasX = ba.getElementAtIndex(0);
2577 final double biasY = ba.getElementAtIndex(1);
2578 final double biasZ = ba.getElementAtIndex(2);
2579
2580 final Matrix ma = generateMaCommonAxis();
2581 final double sx = ma.getElementAt(0, 0);
2582 final double sy = ma.getElementAt(1, 1);
2583 final double sz = ma.getElementAt(2, 2);
2584
2585 final KnownPositionAccelerometerCalibrator calibrator =
2586 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
2587 sx, sy, sz);
2588
2589
2590 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2591 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2592 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2593 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2594 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2595 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2596 final Acceleration bx2 = new Acceleration(0.0,
2597 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2598 calibrator.getInitialBiasXAsAcceleration(bx2);
2599 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2600 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2601 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2602 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2603 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2604 final Acceleration by2 = new Acceleration(0.0,
2605 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2606 calibrator.getInitialBiasYAsAcceleration(by2);
2607 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2608 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2609 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2610 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2611 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2612 final Acceleration bz2 = new Acceleration(0.0,
2613 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2614 calibrator.getInitialBiasZAsAcceleration(bz2);
2615 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2616 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2617 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2618 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2619 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2620 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2621 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2622 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2623 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2624 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2625 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2626 final double[] bias1 = calibrator.getInitialBias();
2627 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2628 final double[] bias2 = new double[3];
2629 calibrator.getInitialBias(bias2);
2630 assertArrayEquals(bias1, bias2, 0.0);
2631 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2632 assertEquals(b1, ba);
2633 final Matrix b2 = new Matrix(3, 1);
2634 calibrator.getInitialBiasAsMatrix(b2);
2635 assertEquals(b1, b2);
2636 final Matrix ma1 = calibrator.getInitialMa();
2637 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2638 final Matrix ma2 = new Matrix(3, 3);
2639 calibrator.getInitialMa(ma2);
2640 assertEquals(ma1, ma2);
2641 assertNull(calibrator.getEcefPosition());
2642 assertNull(calibrator.getNedPosition());
2643 assertFalse(calibrator.getNedPosition(null));
2644 assertNull(calibrator.getMeasurements());
2645 assertFalse(calibrator.isCommonAxisUsed());
2646 assertNull(calibrator.getListener());
2647 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2648 assertFalse(calibrator.isReady());
2649 assertFalse(calibrator.isRunning());
2650 assertNull(calibrator.getEstimatedBiases());
2651 assertFalse(calibrator.getEstimatedBiases(null));
2652 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2653 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2654 assertNull(calibrator.getEstimatedBiasFx());
2655 assertNull(calibrator.getEstimatedBiasFy());
2656 assertNull(calibrator.getEstimatedBiasFz());
2657 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2658 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2659 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2660 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2661 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2662 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2663 assertNull(calibrator.getEstimatedMa());
2664 assertNull(calibrator.getEstimatedSx());
2665 assertNull(calibrator.getEstimatedSy());
2666 assertNull(calibrator.getEstimatedSz());
2667 assertNull(calibrator.getEstimatedMxy());
2668 assertNull(calibrator.getEstimatedMxz());
2669 assertNull(calibrator.getEstimatedMyx());
2670 assertNull(calibrator.getEstimatedMyz());
2671 assertNull(calibrator.getEstimatedMzx());
2672 assertNull(calibrator.getEstimatedMzy());
2673 assertNull(calibrator.getEstimatedCovariance());
2674 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2675 assertNull(calibrator.getGroundTruthGravityNorm());
2676 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2677 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2678 }
2679
2680 @Test
2681 public void testConstructor26() throws WrongSizeException {
2682 final Collection<StandardDeviationBodyKinematics> measurements =
2683 Collections.emptyList();
2684
2685 final Matrix ba = generateBa();
2686 final double biasX = ba.getElementAtIndex(0);
2687 final double biasY = ba.getElementAtIndex(1);
2688 final double biasZ = ba.getElementAtIndex(2);
2689
2690 final Matrix ma = generateMaCommonAxis();
2691 final double sx = ma.getElementAt(0, 0);
2692 final double sy = ma.getElementAt(1, 1);
2693 final double sz = ma.getElementAt(2, 2);
2694
2695 final KnownPositionAccelerometerCalibrator calibrator =
2696 new KnownPositionAccelerometerCalibrator(measurements,
2697 biasX, biasY, biasZ, sx, sy, sz);
2698
2699
2700 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2701 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2702 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2703 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2704 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2705 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2706 final Acceleration bx2 = new Acceleration(0.0,
2707 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2708 calibrator.getInitialBiasXAsAcceleration(bx2);
2709 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2710 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2711 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2712 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2713 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2714 final Acceleration by2 = new Acceleration(0.0,
2715 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2716 calibrator.getInitialBiasYAsAcceleration(by2);
2717 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2718 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2719 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2720 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2721 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2722 final Acceleration bz2 = new Acceleration(0.0,
2723 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2724 calibrator.getInitialBiasZAsAcceleration(bz2);
2725 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2726 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2727 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2728 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2729 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2730 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2731 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2732 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2733 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2734 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2735 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2736 final double[] bias1 = calibrator.getInitialBias();
2737 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2738 final double[] bias2 = new double[3];
2739 calibrator.getInitialBias(bias2);
2740 assertArrayEquals(bias1, bias2, 0.0);
2741 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2742 assertEquals(b1, ba);
2743 final Matrix b2 = new Matrix(3, 1);
2744 calibrator.getInitialBiasAsMatrix(b2);
2745 assertEquals(b1, b2);
2746 final Matrix ma1 = calibrator.getInitialMa();
2747 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2748 final Matrix ma2 = new Matrix(3, 3);
2749 calibrator.getInitialMa(ma2);
2750 assertEquals(ma1, ma2);
2751 assertNull(calibrator.getEcefPosition());
2752 assertNull(calibrator.getNedPosition());
2753 assertFalse(calibrator.getNedPosition(null));
2754 assertSame(calibrator.getMeasurements(), measurements);
2755 assertFalse(calibrator.isCommonAxisUsed());
2756 assertNull(calibrator.getListener());
2757 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2758 assertFalse(calibrator.isReady());
2759 assertFalse(calibrator.isRunning());
2760 assertNull(calibrator.getEstimatedBiases());
2761 assertFalse(calibrator.getEstimatedBiases(null));
2762 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2763 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2764 assertNull(calibrator.getEstimatedBiasFx());
2765 assertNull(calibrator.getEstimatedBiasFy());
2766 assertNull(calibrator.getEstimatedBiasFz());
2767 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2768 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2769 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2770 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2771 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2772 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2773 assertNull(calibrator.getEstimatedMa());
2774 assertNull(calibrator.getEstimatedSx());
2775 assertNull(calibrator.getEstimatedSy());
2776 assertNull(calibrator.getEstimatedSz());
2777 assertNull(calibrator.getEstimatedMxy());
2778 assertNull(calibrator.getEstimatedMxz());
2779 assertNull(calibrator.getEstimatedMyx());
2780 assertNull(calibrator.getEstimatedMyz());
2781 assertNull(calibrator.getEstimatedMzx());
2782 assertNull(calibrator.getEstimatedMzy());
2783 assertNull(calibrator.getEstimatedCovariance());
2784 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2785 assertNull(calibrator.getGroundTruthGravityNorm());
2786 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2787 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2788 }
2789
2790 @Test
2791 public void testConstructor27() throws WrongSizeException {
2792 final Collection<StandardDeviationBodyKinematics> measurements =
2793 Collections.emptyList();
2794
2795 final Matrix ba = generateBa();
2796 final double biasX = ba.getElementAtIndex(0);
2797 final double biasY = ba.getElementAtIndex(1);
2798 final double biasZ = ba.getElementAtIndex(2);
2799
2800 final Matrix ma = generateMaCommonAxis();
2801 final double sx = ma.getElementAt(0, 0);
2802 final double sy = ma.getElementAt(1, 1);
2803 final double sz = ma.getElementAt(2, 2);
2804
2805 final KnownPositionAccelerometerCalibrator calibrator =
2806 new KnownPositionAccelerometerCalibrator(measurements,
2807 biasX, biasY, biasZ, sx, sy, sz, this);
2808
2809
2810 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2811 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2812 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2813 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2814 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2815 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2816 final Acceleration bx2 = new Acceleration(0.0,
2817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2818 calibrator.getInitialBiasXAsAcceleration(bx2);
2819 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2820 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2821 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2822 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2823 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2824 final Acceleration by2 = new Acceleration(0.0,
2825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2826 calibrator.getInitialBiasYAsAcceleration(by2);
2827 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2828 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2829 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2830 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2831 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2832 final Acceleration bz2 = new Acceleration(0.0,
2833 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2834 calibrator.getInitialBiasZAsAcceleration(bz2);
2835 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2836 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2837 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2838 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2839 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2840 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2841 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2842 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2843 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2844 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2845 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2846 final double[] bias1 = calibrator.getInitialBias();
2847 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2848 final double[] bias2 = new double[3];
2849 calibrator.getInitialBias(bias2);
2850 assertArrayEquals(bias1, bias2, 0.0);
2851 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2852 assertEquals(b1, ba);
2853 final Matrix b2 = new Matrix(3, 1);
2854 calibrator.getInitialBiasAsMatrix(b2);
2855 assertEquals(b1, b2);
2856 final Matrix ma1 = calibrator.getInitialMa();
2857 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2858 final Matrix ma2 = new Matrix(3, 3);
2859 calibrator.getInitialMa(ma2);
2860 assertEquals(ma1, ma2);
2861 assertNull(calibrator.getEcefPosition());
2862 assertNull(calibrator.getNedPosition());
2863 assertFalse(calibrator.getNedPosition(null));
2864 assertSame(calibrator.getMeasurements(), measurements);
2865 assertFalse(calibrator.isCommonAxisUsed());
2866 assertSame(calibrator.getListener(), this);
2867 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
2868 assertFalse(calibrator.isReady());
2869 assertFalse(calibrator.isRunning());
2870 assertNull(calibrator.getEstimatedBiases());
2871 assertFalse(calibrator.getEstimatedBiases(null));
2872 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2873 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2874 assertNull(calibrator.getEstimatedBiasFx());
2875 assertNull(calibrator.getEstimatedBiasFy());
2876 assertNull(calibrator.getEstimatedBiasFz());
2877 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2878 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2879 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2880 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2881 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2882 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2883 assertNull(calibrator.getEstimatedMa());
2884 assertNull(calibrator.getEstimatedSx());
2885 assertNull(calibrator.getEstimatedSy());
2886 assertNull(calibrator.getEstimatedSz());
2887 assertNull(calibrator.getEstimatedMxy());
2888 assertNull(calibrator.getEstimatedMxz());
2889 assertNull(calibrator.getEstimatedMyx());
2890 assertNull(calibrator.getEstimatedMyz());
2891 assertNull(calibrator.getEstimatedMzx());
2892 assertNull(calibrator.getEstimatedMzy());
2893 assertNull(calibrator.getEstimatedCovariance());
2894 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
2895 assertNull(calibrator.getGroundTruthGravityNorm());
2896 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
2897 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
2898 }
2899
2900 @Test
2901 public void testConstructor28() throws WrongSizeException {
2902 final Matrix ba = generateBa();
2903 final double biasX = ba.getElementAtIndex(0);
2904 final double biasY = ba.getElementAtIndex(1);
2905 final double biasZ = ba.getElementAtIndex(2);
2906
2907 final Matrix ma = generateMaCommonAxis();
2908 final double sx = ma.getElementAt(0, 0);
2909 final double sy = ma.getElementAt(1, 1);
2910 final double sz = ma.getElementAt(2, 2);
2911
2912 final KnownPositionAccelerometerCalibrator calibrator =
2913 new KnownPositionAccelerometerCalibrator(true,
2914 biasX, biasY, biasZ, sx, sy, sz);
2915
2916
2917 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
2918 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
2919 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
2920 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
2921 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
2922 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2923 final Acceleration bx2 = new Acceleration(0.0,
2924 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2925 calibrator.getInitialBiasXAsAcceleration(bx2);
2926 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
2927 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2928 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
2929 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
2930 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2931 final Acceleration by2 = new Acceleration(0.0,
2932 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2933 calibrator.getInitialBiasYAsAcceleration(by2);
2934 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
2935 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2936 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
2937 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
2938 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2939 final Acceleration bz2 = new Acceleration(0.0,
2940 AccelerationUnit.FEET_PER_SQUARED_SECOND);
2941 calibrator.getInitialBiasZAsAcceleration(bz2);
2942 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
2943 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
2944 assertEquals(calibrator.getInitialSx(), sx, 0.0);
2945 assertEquals(calibrator.getInitialSy(), sy, 0.0);
2946 assertEquals(calibrator.getInitialSz(), sz, 0.0);
2947 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
2948 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
2949 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
2950 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
2951 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
2952 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
2953 final double[] bias1 = calibrator.getInitialBias();
2954 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
2955 final double[] bias2 = new double[3];
2956 calibrator.getInitialBias(bias2);
2957 assertArrayEquals(bias1, bias2, 0.0);
2958 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
2959 assertEquals(b1, ba);
2960 final Matrix b2 = new Matrix(3, 1);
2961 calibrator.getInitialBiasAsMatrix(b2);
2962 assertEquals(b1, b2);
2963 final Matrix ma1 = calibrator.getInitialMa();
2964 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
2965 final Matrix ma2 = new Matrix(3, 3);
2966 calibrator.getInitialMa(ma2);
2967 assertEquals(ma1, ma2);
2968 assertNull(calibrator.getEcefPosition());
2969 assertNull(calibrator.getNedPosition());
2970 assertFalse(calibrator.getNedPosition(null));
2971 assertNull(calibrator.getMeasurements());
2972 assertTrue(calibrator.isCommonAxisUsed());
2973 assertNull(calibrator.getListener());
2974 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
2975 assertFalse(calibrator.isReady());
2976 assertFalse(calibrator.isRunning());
2977 assertNull(calibrator.getEstimatedBiases());
2978 assertFalse(calibrator.getEstimatedBiases(null));
2979 assertNull(calibrator.getEstimatedBiasesAsMatrix());
2980 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
2981 assertNull(calibrator.getEstimatedBiasFx());
2982 assertNull(calibrator.getEstimatedBiasFy());
2983 assertNull(calibrator.getEstimatedBiasFz());
2984 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
2985 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
2986 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
2987 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
2988 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
2989 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
2990 assertNull(calibrator.getEstimatedMa());
2991 assertNull(calibrator.getEstimatedSx());
2992 assertNull(calibrator.getEstimatedSy());
2993 assertNull(calibrator.getEstimatedSz());
2994 assertNull(calibrator.getEstimatedMxy());
2995 assertNull(calibrator.getEstimatedMxz());
2996 assertNull(calibrator.getEstimatedMyx());
2997 assertNull(calibrator.getEstimatedMyz());
2998 assertNull(calibrator.getEstimatedMzx());
2999 assertNull(calibrator.getEstimatedMzy());
3000 assertNull(calibrator.getEstimatedCovariance());
3001 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3002 assertNull(calibrator.getGroundTruthGravityNorm());
3003 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3004 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3005 }
3006
3007 @Test
3008 public void testConstructor29() throws WrongSizeException {
3009 final Matrix ba = generateBa();
3010 final double biasX = ba.getElementAtIndex(0);
3011 final double biasY = ba.getElementAtIndex(1);
3012 final double biasZ = ba.getElementAtIndex(2);
3013
3014 final Matrix ma = generateMaCommonAxis();
3015 final double sx = ma.getElementAt(0, 0);
3016 final double sy = ma.getElementAt(1, 1);
3017 final double sz = ma.getElementAt(2, 2);
3018
3019 final KnownPositionAccelerometerCalibrator calibrator =
3020 new KnownPositionAccelerometerCalibrator(true,
3021 biasX, biasY, biasZ, sx, sy, sz, this);
3022
3023
3024 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3025 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3026 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3027 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3028 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3029 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3030 final Acceleration bx2 = new Acceleration(0.0,
3031 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3032 calibrator.getInitialBiasXAsAcceleration(bx2);
3033 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3034 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3035 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3036 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3037 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3038 final Acceleration by2 = new Acceleration(0.0,
3039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3040 calibrator.getInitialBiasYAsAcceleration(by2);
3041 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3042 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3043 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3044 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3045 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3046 final Acceleration bz2 = new Acceleration(0.0,
3047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3048 calibrator.getInitialBiasZAsAcceleration(bz2);
3049 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3050 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3051 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3052 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3053 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3054 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3055 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3056 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3057 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3058 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3059 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3060 final double[] bias1 = calibrator.getInitialBias();
3061 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3062 final double[] bias2 = new double[3];
3063 calibrator.getInitialBias(bias2);
3064 assertArrayEquals(bias1, bias2, 0.0);
3065 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3066 assertEquals(b1, ba);
3067 final Matrix b2 = new Matrix(3, 1);
3068 calibrator.getInitialBiasAsMatrix(b2);
3069 assertEquals(b1, b2);
3070 final Matrix ma1 = calibrator.getInitialMa();
3071 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3072 final Matrix ma2 = new Matrix(3, 3);
3073 calibrator.getInitialMa(ma2);
3074 assertEquals(ma1, ma2);
3075 assertNull(calibrator.getEcefPosition());
3076 assertNull(calibrator.getNedPosition());
3077 assertFalse(calibrator.getNedPosition(null));
3078 assertNull(calibrator.getMeasurements());
3079 assertTrue(calibrator.isCommonAxisUsed());
3080 assertSame(calibrator.getListener(), this);
3081 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3082 assertFalse(calibrator.isReady());
3083 assertFalse(calibrator.isRunning());
3084 assertNull(calibrator.getEstimatedBiases());
3085 assertFalse(calibrator.getEstimatedBiases(null));
3086 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3087 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3088 assertNull(calibrator.getEstimatedBiasFx());
3089 assertNull(calibrator.getEstimatedBiasFy());
3090 assertNull(calibrator.getEstimatedBiasFz());
3091 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3092 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3093 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3094 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3095 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3096 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3097 assertNull(calibrator.getEstimatedMa());
3098 assertNull(calibrator.getEstimatedSx());
3099 assertNull(calibrator.getEstimatedSy());
3100 assertNull(calibrator.getEstimatedSz());
3101 assertNull(calibrator.getEstimatedMxy());
3102 assertNull(calibrator.getEstimatedMxz());
3103 assertNull(calibrator.getEstimatedMyx());
3104 assertNull(calibrator.getEstimatedMyz());
3105 assertNull(calibrator.getEstimatedMzx());
3106 assertNull(calibrator.getEstimatedMzy());
3107 assertNull(calibrator.getEstimatedCovariance());
3108 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3109 assertNull(calibrator.getGroundTruthGravityNorm());
3110 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3111 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3112 }
3113
3114 @Test
3115 public void testConstructor30() throws WrongSizeException {
3116 final Collection<StandardDeviationBodyKinematics> measurements =
3117 Collections.emptyList();
3118 final Matrix ba = generateBa();
3119 final double biasX = ba.getElementAtIndex(0);
3120 final double biasY = ba.getElementAtIndex(1);
3121 final double biasZ = ba.getElementAtIndex(2);
3122
3123 final Matrix ma = generateMaCommonAxis();
3124 final double sx = ma.getElementAt(0, 0);
3125 final double sy = ma.getElementAt(1, 1);
3126 final double sz = ma.getElementAt(2, 2);
3127
3128 final KnownPositionAccelerometerCalibrator calibrator =
3129 new KnownPositionAccelerometerCalibrator(measurements,
3130 true, biasX, biasY, biasZ, sx, sy, sz);
3131
3132
3133 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3134 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3135 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3136 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3137 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3138 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3139 final Acceleration bx2 = new Acceleration(0.0,
3140 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3141 calibrator.getInitialBiasXAsAcceleration(bx2);
3142 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3143 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3144 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3145 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3146 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3147 final Acceleration by2 = new Acceleration(0.0,
3148 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3149 calibrator.getInitialBiasYAsAcceleration(by2);
3150 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3151 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3152 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3153 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3154 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3155 final Acceleration bz2 = new Acceleration(0.0,
3156 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3157 calibrator.getInitialBiasZAsAcceleration(bz2);
3158 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3159 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3160 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3161 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3162 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3163 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3164 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3165 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3166 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3167 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3168 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3169 final double[] bias1 = calibrator.getInitialBias();
3170 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3171 final double[] bias2 = new double[3];
3172 calibrator.getInitialBias(bias2);
3173 assertArrayEquals(bias1, bias2, 0.0);
3174 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3175 assertEquals(b1, ba);
3176 final Matrix b2 = new Matrix(3, 1);
3177 calibrator.getInitialBiasAsMatrix(b2);
3178 assertEquals(b1, b2);
3179 final Matrix ma1 = calibrator.getInitialMa();
3180 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3181 final Matrix ma2 = new Matrix(3, 3);
3182 calibrator.getInitialMa(ma2);
3183 assertEquals(ma1, ma2);
3184 assertNull(calibrator.getEcefPosition());
3185 assertNull(calibrator.getNedPosition());
3186 assertFalse(calibrator.getNedPosition(null));
3187 assertSame(calibrator.getMeasurements(), measurements);
3188 assertTrue(calibrator.isCommonAxisUsed());
3189 assertNull(calibrator.getListener());
3190 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3191 assertFalse(calibrator.isReady());
3192 assertFalse(calibrator.isRunning());
3193 assertNull(calibrator.getEstimatedBiases());
3194 assertFalse(calibrator.getEstimatedBiases(null));
3195 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3196 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3197 assertNull(calibrator.getEstimatedBiasFx());
3198 assertNull(calibrator.getEstimatedBiasFy());
3199 assertNull(calibrator.getEstimatedBiasFz());
3200 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3201 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3202 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3203 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3204 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3205 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3206 assertNull(calibrator.getEstimatedMa());
3207 assertNull(calibrator.getEstimatedSx());
3208 assertNull(calibrator.getEstimatedSy());
3209 assertNull(calibrator.getEstimatedSz());
3210 assertNull(calibrator.getEstimatedMxy());
3211 assertNull(calibrator.getEstimatedMxz());
3212 assertNull(calibrator.getEstimatedMyx());
3213 assertNull(calibrator.getEstimatedMyz());
3214 assertNull(calibrator.getEstimatedMzx());
3215 assertNull(calibrator.getEstimatedMzy());
3216 assertNull(calibrator.getEstimatedCovariance());
3217 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3218 assertNull(calibrator.getGroundTruthGravityNorm());
3219 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3220 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3221 }
3222
3223 @Test
3224 public void testConstructor31() throws WrongSizeException {
3225 final Collection<StandardDeviationBodyKinematics> measurements =
3226 Collections.emptyList();
3227 final Matrix ba = generateBa();
3228 final double biasX = ba.getElementAtIndex(0);
3229 final double biasY = ba.getElementAtIndex(1);
3230 final double biasZ = ba.getElementAtIndex(2);
3231
3232 final Matrix ma = generateMaCommonAxis();
3233 final double sx = ma.getElementAt(0, 0);
3234 final double sy = ma.getElementAt(1, 1);
3235 final double sz = ma.getElementAt(2, 2);
3236
3237 final KnownPositionAccelerometerCalibrator calibrator =
3238 new KnownPositionAccelerometerCalibrator(measurements,
3239 true, biasX, biasY, biasZ, sx, sy, sz,
3240 this);
3241
3242
3243 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3244 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3245 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3246 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3247 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3248 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3249 final Acceleration bx2 = new Acceleration(0.0,
3250 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3251 calibrator.getInitialBiasXAsAcceleration(bx2);
3252 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3253 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3254 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3255 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3256 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3257 final Acceleration by2 = new Acceleration(0.0,
3258 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3259 calibrator.getInitialBiasYAsAcceleration(by2);
3260 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3261 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3262 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3263 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3264 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3265 final Acceleration bz2 = new Acceleration(0.0,
3266 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3267 calibrator.getInitialBiasZAsAcceleration(bz2);
3268 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3269 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3270 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3271 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3272 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3273 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3274 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3275 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3276 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3277 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3278 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3279 final double[] bias1 = calibrator.getInitialBias();
3280 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3281 final double[] bias2 = new double[3];
3282 calibrator.getInitialBias(bias2);
3283 assertArrayEquals(bias1, bias2, 0.0);
3284 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3285 assertEquals(b1, ba);
3286 final Matrix b2 = new Matrix(3, 1);
3287 calibrator.getInitialBiasAsMatrix(b2);
3288 assertEquals(b1, b2);
3289 final Matrix ma1 = calibrator.getInitialMa();
3290 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3291 final Matrix ma2 = new Matrix(3, 3);
3292 calibrator.getInitialMa(ma2);
3293 assertEquals(ma1, ma2);
3294 assertNull(calibrator.getEcefPosition());
3295 assertNull(calibrator.getNedPosition());
3296 assertFalse(calibrator.getNedPosition(null));
3297 assertSame(calibrator.getMeasurements(), measurements);
3298 assertTrue(calibrator.isCommonAxisUsed());
3299 assertSame(calibrator.getListener(), this);
3300 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3301 assertFalse(calibrator.isReady());
3302 assertFalse(calibrator.isRunning());
3303 assertNull(calibrator.getEstimatedBiases());
3304 assertFalse(calibrator.getEstimatedBiases(null));
3305 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3306 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3307 assertNull(calibrator.getEstimatedBiasFx());
3308 assertNull(calibrator.getEstimatedBiasFy());
3309 assertNull(calibrator.getEstimatedBiasFz());
3310 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3311 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3312 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3313 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3314 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3315 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3316 assertNull(calibrator.getEstimatedMa());
3317 assertNull(calibrator.getEstimatedSx());
3318 assertNull(calibrator.getEstimatedSy());
3319 assertNull(calibrator.getEstimatedSz());
3320 assertNull(calibrator.getEstimatedMxy());
3321 assertNull(calibrator.getEstimatedMxz());
3322 assertNull(calibrator.getEstimatedMyx());
3323 assertNull(calibrator.getEstimatedMyz());
3324 assertNull(calibrator.getEstimatedMzx());
3325 assertNull(calibrator.getEstimatedMzy());
3326 assertNull(calibrator.getEstimatedCovariance());
3327 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3328 assertNull(calibrator.getGroundTruthGravityNorm());
3329 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3330 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3331 }
3332
3333 @Test
3334 public void testConstructor32() throws WrongSizeException {
3335 final Matrix ba = generateBa();
3336 final double biasX = ba.getElementAtIndex(0);
3337 final double biasY = ba.getElementAtIndex(1);
3338 final double biasZ = ba.getElementAtIndex(2);
3339
3340 final Matrix ma = generateMaCommonAxis();
3341 final double sx = ma.getElementAt(0, 0);
3342 final double sy = ma.getElementAt(1, 1);
3343 final double sz = ma.getElementAt(2, 2);
3344
3345 final Acceleration bx = new Acceleration(biasX,
3346 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3347 final Acceleration by = new Acceleration(biasY,
3348 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3349 final Acceleration bz = new Acceleration(biasZ,
3350 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3351
3352 final KnownPositionAccelerometerCalibrator calibrator =
3353 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3354 sx, sy, sz);
3355
3356
3357 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3358 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3359 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3360 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3361 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3362 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3363 final Acceleration bx2 = new Acceleration(0.0,
3364 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3365 calibrator.getInitialBiasXAsAcceleration(bx2);
3366 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3367 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3368 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3369 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3370 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3371 final Acceleration by2 = new Acceleration(0.0,
3372 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3373 calibrator.getInitialBiasYAsAcceleration(by2);
3374 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3375 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3376 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3377 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3378 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3379 final Acceleration bz2 = new Acceleration(0.0,
3380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3381 calibrator.getInitialBiasZAsAcceleration(bz2);
3382 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3383 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3384 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3385 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3386 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3387 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3388 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3389 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3390 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3391 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3392 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3393 final double[] bias1 = calibrator.getInitialBias();
3394 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3395 final double[] bias2 = new double[3];
3396 calibrator.getInitialBias(bias2);
3397 assertArrayEquals(bias1, bias2, 0.0);
3398 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3399 assertEquals(b1, ba);
3400 final Matrix b2 = new Matrix(3, 1);
3401 calibrator.getInitialBiasAsMatrix(b2);
3402 assertEquals(b1, b2);
3403 final Matrix ma1 = calibrator.getInitialMa();
3404 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3405 final Matrix ma2 = new Matrix(3, 3);
3406 calibrator.getInitialMa(ma2);
3407 assertEquals(ma1, ma2);
3408 assertNull(calibrator.getEcefPosition());
3409 assertNull(calibrator.getNedPosition());
3410 assertFalse(calibrator.getNedPosition(null));
3411 assertNull(calibrator.getMeasurements());
3412 assertFalse(calibrator.isCommonAxisUsed());
3413 assertNull(calibrator.getListener());
3414 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3415 assertFalse(calibrator.isReady());
3416 assertFalse(calibrator.isRunning());
3417 assertNull(calibrator.getEstimatedBiases());
3418 assertFalse(calibrator.getEstimatedBiases(null));
3419 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3420 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3421 assertNull(calibrator.getEstimatedBiasFx());
3422 assertNull(calibrator.getEstimatedBiasFy());
3423 assertNull(calibrator.getEstimatedBiasFz());
3424 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3425 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3426 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3427 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3428 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3429 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3430 assertNull(calibrator.getEstimatedMa());
3431 assertNull(calibrator.getEstimatedSx());
3432 assertNull(calibrator.getEstimatedSy());
3433 assertNull(calibrator.getEstimatedSz());
3434 assertNull(calibrator.getEstimatedMxy());
3435 assertNull(calibrator.getEstimatedMxz());
3436 assertNull(calibrator.getEstimatedMyx());
3437 assertNull(calibrator.getEstimatedMyz());
3438 assertNull(calibrator.getEstimatedMzx());
3439 assertNull(calibrator.getEstimatedMzy());
3440 assertNull(calibrator.getEstimatedCovariance());
3441 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3442 assertNull(calibrator.getGroundTruthGravityNorm());
3443 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3444 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3445 }
3446
3447 @Test
3448 public void testConstructor33() throws WrongSizeException {
3449 final Matrix ba = generateBa();
3450 final double biasX = ba.getElementAtIndex(0);
3451 final double biasY = ba.getElementAtIndex(1);
3452 final double biasZ = ba.getElementAtIndex(2);
3453
3454 final Matrix ma = generateMaCommonAxis();
3455 final double sx = ma.getElementAt(0, 0);
3456 final double sy = ma.getElementAt(1, 1);
3457 final double sz = ma.getElementAt(2, 2);
3458
3459 final Acceleration bx = new Acceleration(biasX,
3460 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3461 final Acceleration by = new Acceleration(biasY,
3462 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3463 final Acceleration bz = new Acceleration(biasZ,
3464 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3465
3466 final KnownPositionAccelerometerCalibrator calibrator =
3467 new KnownPositionAccelerometerCalibrator(bx, by, bz,
3468 sx, sy, sz, this);
3469
3470
3471 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3472 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3473 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3474 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3475 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3476 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3477 final Acceleration bx2 = new Acceleration(0.0,
3478 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3479 calibrator.getInitialBiasXAsAcceleration(bx2);
3480 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3481 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3482 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3483 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3484 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3485 final Acceleration by2 = new Acceleration(0.0,
3486 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3487 calibrator.getInitialBiasYAsAcceleration(by2);
3488 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3489 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3490 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3491 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3492 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3493 final Acceleration bz2 = new Acceleration(0.0,
3494 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3495 calibrator.getInitialBiasZAsAcceleration(bz2);
3496 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3497 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3498 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3499 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3500 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3501 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3502 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3503 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3504 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3505 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3506 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3507 final double[] bias1 = calibrator.getInitialBias();
3508 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3509 final double[] bias2 = new double[3];
3510 calibrator.getInitialBias(bias2);
3511 assertArrayEquals(bias1, bias2, 0.0);
3512 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3513 assertEquals(b1, ba);
3514 final Matrix b2 = new Matrix(3, 1);
3515 calibrator.getInitialBiasAsMatrix(b2);
3516 assertEquals(b1, b2);
3517 final Matrix ma1 = calibrator.getInitialMa();
3518 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3519 final Matrix ma2 = new Matrix(3, 3);
3520 calibrator.getInitialMa(ma2);
3521 assertEquals(ma1, ma2);
3522 assertNull(calibrator.getEcefPosition());
3523 assertNull(calibrator.getNedPosition());
3524 assertFalse(calibrator.getNedPosition(null));
3525 assertNull(calibrator.getMeasurements());
3526 assertFalse(calibrator.isCommonAxisUsed());
3527 assertSame(calibrator.getListener(), this);
3528 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3529 assertFalse(calibrator.isReady());
3530 assertFalse(calibrator.isRunning());
3531 assertNull(calibrator.getEstimatedBiases());
3532 assertFalse(calibrator.getEstimatedBiases(null));
3533 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3534 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3535 assertNull(calibrator.getEstimatedBiasFx());
3536 assertNull(calibrator.getEstimatedBiasFy());
3537 assertNull(calibrator.getEstimatedBiasFz());
3538 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3539 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3540 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3541 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3542 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3543 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3544 assertNull(calibrator.getEstimatedMa());
3545 assertNull(calibrator.getEstimatedSx());
3546 assertNull(calibrator.getEstimatedSy());
3547 assertNull(calibrator.getEstimatedSz());
3548 assertNull(calibrator.getEstimatedMxy());
3549 assertNull(calibrator.getEstimatedMxz());
3550 assertNull(calibrator.getEstimatedMyx());
3551 assertNull(calibrator.getEstimatedMyz());
3552 assertNull(calibrator.getEstimatedMzx());
3553 assertNull(calibrator.getEstimatedMzy());
3554 assertNull(calibrator.getEstimatedCovariance());
3555 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3556 assertNull(calibrator.getGroundTruthGravityNorm());
3557 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3558 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3559 }
3560
3561 @Test
3562 public void testConstructor34() throws WrongSizeException {
3563 final Collection<StandardDeviationBodyKinematics> measurements =
3564 Collections.emptyList();
3565
3566 final Matrix ba = generateBa();
3567 final double biasX = ba.getElementAtIndex(0);
3568 final double biasY = ba.getElementAtIndex(1);
3569 final double biasZ = ba.getElementAtIndex(2);
3570
3571 final Matrix ma = generateMaCommonAxis();
3572 final double sx = ma.getElementAt(0, 0);
3573 final double sy = ma.getElementAt(1, 1);
3574 final double sz = ma.getElementAt(2, 2);
3575
3576 final Acceleration bx = new Acceleration(biasX,
3577 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3578 final Acceleration by = new Acceleration(biasY,
3579 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3580 final Acceleration bz = new Acceleration(biasZ,
3581 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3582
3583 final KnownPositionAccelerometerCalibrator calibrator =
3584 new KnownPositionAccelerometerCalibrator(measurements,
3585 bx, by, bz, sx, sy, sz);
3586
3587
3588 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3589 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3590 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3591 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3592 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3593 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3594 final Acceleration bx2 = new Acceleration(0.0,
3595 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3596 calibrator.getInitialBiasXAsAcceleration(bx2);
3597 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3598 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3599 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3600 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3601 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3602 final Acceleration by2 = new Acceleration(0.0,
3603 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3604 calibrator.getInitialBiasYAsAcceleration(by2);
3605 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3606 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3607 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3608 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3609 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3610 final Acceleration bz2 = new Acceleration(0.0,
3611 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3612 calibrator.getInitialBiasZAsAcceleration(bz2);
3613 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3614 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3615 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3616 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3617 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3618 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3619 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3620 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3621 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3622 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3623 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3624 final double[] bias1 = calibrator.getInitialBias();
3625 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3626 final double[] bias2 = new double[3];
3627 calibrator.getInitialBias(bias2);
3628 assertArrayEquals(bias1, bias2, 0.0);
3629 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3630 assertEquals(b1, ba);
3631 final Matrix b2 = new Matrix(3, 1);
3632 calibrator.getInitialBiasAsMatrix(b2);
3633 assertEquals(b1, b2);
3634 final Matrix ma1 = calibrator.getInitialMa();
3635 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3636 final Matrix ma2 = new Matrix(3, 3);
3637 calibrator.getInitialMa(ma2);
3638 assertEquals(ma1, ma2);
3639 assertNull(calibrator.getEcefPosition());
3640 assertNull(calibrator.getNedPosition());
3641 assertFalse(calibrator.getNedPosition(null));
3642 assertSame(calibrator.getMeasurements(), measurements);
3643 assertFalse(calibrator.isCommonAxisUsed());
3644 assertNull(calibrator.getListener());
3645 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3646 assertFalse(calibrator.isReady());
3647 assertFalse(calibrator.isRunning());
3648 assertNull(calibrator.getEstimatedBiases());
3649 assertFalse(calibrator.getEstimatedBiases(null));
3650 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3651 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3652 assertNull(calibrator.getEstimatedBiasFx());
3653 assertNull(calibrator.getEstimatedBiasFy());
3654 assertNull(calibrator.getEstimatedBiasFz());
3655 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3656 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3657 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3658 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3659 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3660 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3661 assertNull(calibrator.getEstimatedMa());
3662 assertNull(calibrator.getEstimatedSx());
3663 assertNull(calibrator.getEstimatedSy());
3664 assertNull(calibrator.getEstimatedSz());
3665 assertNull(calibrator.getEstimatedMxy());
3666 assertNull(calibrator.getEstimatedMxz());
3667 assertNull(calibrator.getEstimatedMyx());
3668 assertNull(calibrator.getEstimatedMyz());
3669 assertNull(calibrator.getEstimatedMzx());
3670 assertNull(calibrator.getEstimatedMzy());
3671 assertNull(calibrator.getEstimatedCovariance());
3672 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3673 assertNull(calibrator.getGroundTruthGravityNorm());
3674 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3675 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3676 }
3677
3678 @Test
3679 public void testConstructor35() throws WrongSizeException {
3680 final Collection<StandardDeviationBodyKinematics> measurements =
3681 Collections.emptyList();
3682
3683 final Matrix ba = generateBa();
3684 final double biasX = ba.getElementAtIndex(0);
3685 final double biasY = ba.getElementAtIndex(1);
3686 final double biasZ = ba.getElementAtIndex(2);
3687
3688 final Matrix ma = generateMaCommonAxis();
3689 final double sx = ma.getElementAt(0, 0);
3690 final double sy = ma.getElementAt(1, 1);
3691 final double sz = ma.getElementAt(2, 2);
3692
3693 final Acceleration bx = new Acceleration(biasX,
3694 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3695 final Acceleration by = new Acceleration(biasY,
3696 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3697 final Acceleration bz = new Acceleration(biasZ,
3698 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3699
3700 final KnownPositionAccelerometerCalibrator calibrator =
3701 new KnownPositionAccelerometerCalibrator(measurements,
3702 bx, by, bz, sx, sy, sz, this);
3703
3704
3705 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3706 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3707 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3708 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3709 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3710 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3711 final Acceleration bx2 = new Acceleration(0.0,
3712 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3713 calibrator.getInitialBiasXAsAcceleration(bx2);
3714 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3715 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3716 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3717 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3718 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3719 final Acceleration by2 = new Acceleration(0.0,
3720 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3721 calibrator.getInitialBiasYAsAcceleration(by2);
3722 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3723 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3724 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3725 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3726 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3727 final Acceleration bz2 = new Acceleration(0.0,
3728 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3729 calibrator.getInitialBiasZAsAcceleration(bz2);
3730 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3731 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3732 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3733 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3734 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3735 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3736 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3737 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3738 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3739 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3740 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3741 final double[] bias1 = calibrator.getInitialBias();
3742 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3743 final double[] bias2 = new double[3];
3744 calibrator.getInitialBias(bias2);
3745 assertArrayEquals(bias1, bias2, 0.0);
3746 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3747 assertEquals(b1, ba);
3748 final Matrix b2 = new Matrix(3, 1);
3749 calibrator.getInitialBiasAsMatrix(b2);
3750 assertEquals(b1, b2);
3751 final Matrix ma1 = calibrator.getInitialMa();
3752 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3753 final Matrix ma2 = new Matrix(3, 3);
3754 calibrator.getInitialMa(ma2);
3755 assertEquals(ma1, ma2);
3756 assertNull(calibrator.getEcefPosition());
3757 assertNull(calibrator.getNedPosition());
3758 assertFalse(calibrator.getNedPosition(null));
3759 assertSame(calibrator.getMeasurements(), measurements);
3760 assertFalse(calibrator.isCommonAxisUsed());
3761 assertSame(calibrator.getListener(), this);
3762 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
3763 assertFalse(calibrator.isReady());
3764 assertFalse(calibrator.isRunning());
3765 assertNull(calibrator.getEstimatedBiases());
3766 assertFalse(calibrator.getEstimatedBiases(null));
3767 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3768 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3769 assertNull(calibrator.getEstimatedBiasFx());
3770 assertNull(calibrator.getEstimatedBiasFy());
3771 assertNull(calibrator.getEstimatedBiasFz());
3772 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3773 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3774 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3775 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3776 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3777 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3778 assertNull(calibrator.getEstimatedMa());
3779 assertNull(calibrator.getEstimatedSx());
3780 assertNull(calibrator.getEstimatedSy());
3781 assertNull(calibrator.getEstimatedSz());
3782 assertNull(calibrator.getEstimatedMxy());
3783 assertNull(calibrator.getEstimatedMxz());
3784 assertNull(calibrator.getEstimatedMyx());
3785 assertNull(calibrator.getEstimatedMyz());
3786 assertNull(calibrator.getEstimatedMzx());
3787 assertNull(calibrator.getEstimatedMzy());
3788 assertNull(calibrator.getEstimatedCovariance());
3789 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3790 assertNull(calibrator.getGroundTruthGravityNorm());
3791 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3792 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3793 }
3794
3795 @Test
3796 public void testConstructor36() throws WrongSizeException {
3797 final Matrix ba = generateBa();
3798 final double biasX = ba.getElementAtIndex(0);
3799 final double biasY = ba.getElementAtIndex(1);
3800 final double biasZ = ba.getElementAtIndex(2);
3801
3802 final Matrix ma = generateMaCommonAxis();
3803 final double sx = ma.getElementAt(0, 0);
3804 final double sy = ma.getElementAt(1, 1);
3805 final double sz = ma.getElementAt(2, 2);
3806
3807 final Acceleration bx = new Acceleration(biasX,
3808 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3809 final Acceleration by = new Acceleration(biasY,
3810 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3811 final Acceleration bz = new Acceleration(biasZ,
3812 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3813
3814 final KnownPositionAccelerometerCalibrator calibrator =
3815 new KnownPositionAccelerometerCalibrator(true,
3816 bx, by, bz, sx, sy, sz);
3817
3818
3819 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3820 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3821 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3822 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3823 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3824 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3825 final Acceleration bx2 = new Acceleration(0.0,
3826 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3827 calibrator.getInitialBiasXAsAcceleration(bx2);
3828 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3829 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3830 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3831 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3832 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3833 final Acceleration by2 = new Acceleration(0.0,
3834 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3835 calibrator.getInitialBiasYAsAcceleration(by2);
3836 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3837 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3838 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3839 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3840 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3841 final Acceleration bz2 = new Acceleration(0.0,
3842 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3843 calibrator.getInitialBiasZAsAcceleration(bz2);
3844 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3845 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3846 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3847 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3848 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3849 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3850 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3851 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3852 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3853 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3854 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3855 final double[] bias1 = calibrator.getInitialBias();
3856 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3857 final double[] bias2 = new double[3];
3858 calibrator.getInitialBias(bias2);
3859 assertArrayEquals(bias1, bias2, 0.0);
3860 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3861 assertEquals(b1, ba);
3862 final Matrix b2 = new Matrix(3, 1);
3863 calibrator.getInitialBiasAsMatrix(b2);
3864 assertEquals(b1, b2);
3865 final Matrix ma1 = calibrator.getInitialMa();
3866 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3867 final Matrix ma2 = new Matrix(3, 3);
3868 calibrator.getInitialMa(ma2);
3869 assertEquals(ma1, ma2);
3870 assertNull(calibrator.getEcefPosition());
3871 assertNull(calibrator.getNedPosition());
3872 assertFalse(calibrator.getNedPosition(null));
3873 assertNull(calibrator.getMeasurements());
3874 assertTrue(calibrator.isCommonAxisUsed());
3875 assertNull(calibrator.getListener());
3876 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3877 assertFalse(calibrator.isReady());
3878 assertFalse(calibrator.isRunning());
3879 assertNull(calibrator.getEstimatedBiases());
3880 assertFalse(calibrator.getEstimatedBiases(null));
3881 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3882 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3883 assertNull(calibrator.getEstimatedBiasFx());
3884 assertNull(calibrator.getEstimatedBiasFy());
3885 assertNull(calibrator.getEstimatedBiasFz());
3886 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
3887 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
3888 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
3889 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
3890 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
3891 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
3892 assertNull(calibrator.getEstimatedMa());
3893 assertNull(calibrator.getEstimatedSx());
3894 assertNull(calibrator.getEstimatedSy());
3895 assertNull(calibrator.getEstimatedSz());
3896 assertNull(calibrator.getEstimatedMxy());
3897 assertNull(calibrator.getEstimatedMxz());
3898 assertNull(calibrator.getEstimatedMyx());
3899 assertNull(calibrator.getEstimatedMyz());
3900 assertNull(calibrator.getEstimatedMzx());
3901 assertNull(calibrator.getEstimatedMzy());
3902 assertNull(calibrator.getEstimatedCovariance());
3903 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
3904 assertNull(calibrator.getGroundTruthGravityNorm());
3905 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
3906 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
3907 }
3908
3909 @Test
3910 public void testConstructor37() throws WrongSizeException {
3911 final Matrix ba = generateBa();
3912 final double biasX = ba.getElementAtIndex(0);
3913 final double biasY = ba.getElementAtIndex(1);
3914 final double biasZ = ba.getElementAtIndex(2);
3915
3916 final Matrix ma = generateMaCommonAxis();
3917 final double sx = ma.getElementAt(0, 0);
3918 final double sy = ma.getElementAt(1, 1);
3919 final double sz = ma.getElementAt(2, 2);
3920
3921 final Acceleration bx = new Acceleration(biasX,
3922 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3923 final Acceleration by = new Acceleration(biasY,
3924 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3925 final Acceleration bz = new Acceleration(biasZ,
3926 AccelerationUnit.METERS_PER_SQUARED_SECOND);
3927
3928 final KnownPositionAccelerometerCalibrator calibrator =
3929 new KnownPositionAccelerometerCalibrator(true,
3930 bx, by, bz, sx, sy, sz, this);
3931
3932
3933 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
3934 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
3935 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
3936 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
3937 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
3938 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3939 final Acceleration bx2 = new Acceleration(0.0,
3940 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3941 calibrator.getInitialBiasXAsAcceleration(bx2);
3942 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
3943 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3944 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
3945 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
3946 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3947 final Acceleration by2 = new Acceleration(0.0,
3948 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3949 calibrator.getInitialBiasYAsAcceleration(by2);
3950 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
3951 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3952 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
3953 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
3954 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3955 final Acceleration bz2 = new Acceleration(0.0,
3956 AccelerationUnit.FEET_PER_SQUARED_SECOND);
3957 calibrator.getInitialBiasZAsAcceleration(bz2);
3958 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
3959 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
3960 assertEquals(calibrator.getInitialSx(), sx, 0.0);
3961 assertEquals(calibrator.getInitialSy(), sy, 0.0);
3962 assertEquals(calibrator.getInitialSz(), sz, 0.0);
3963 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
3964 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
3965 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
3966 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
3967 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
3968 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
3969 final double[] bias1 = calibrator.getInitialBias();
3970 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
3971 final double[] bias2 = new double[3];
3972 calibrator.getInitialBias(bias2);
3973 assertArrayEquals(bias1, bias2, 0.0);
3974 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
3975 assertEquals(b1, ba);
3976 final Matrix b2 = new Matrix(3, 1);
3977 calibrator.getInitialBiasAsMatrix(b2);
3978 assertEquals(b1, b2);
3979 final Matrix ma1 = calibrator.getInitialMa();
3980 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
3981 final Matrix ma2 = new Matrix(3, 3);
3982 calibrator.getInitialMa(ma2);
3983 assertEquals(ma1, ma2);
3984 assertNull(calibrator.getEcefPosition());
3985 assertNull(calibrator.getNedPosition());
3986 assertFalse(calibrator.getNedPosition(null));
3987 assertNull(calibrator.getMeasurements());
3988 assertTrue(calibrator.isCommonAxisUsed());
3989 assertSame(calibrator.getListener(), this);
3990 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
3991 assertFalse(calibrator.isReady());
3992 assertFalse(calibrator.isRunning());
3993 assertNull(calibrator.getEstimatedBiases());
3994 assertFalse(calibrator.getEstimatedBiases(null));
3995 assertNull(calibrator.getEstimatedBiasesAsMatrix());
3996 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
3997 assertNull(calibrator.getEstimatedBiasFx());
3998 assertNull(calibrator.getEstimatedBiasFy());
3999 assertNull(calibrator.getEstimatedBiasFz());
4000 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4001 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4002 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4003 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4004 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4005 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4006 assertNull(calibrator.getEstimatedMa());
4007 assertNull(calibrator.getEstimatedSx());
4008 assertNull(calibrator.getEstimatedSy());
4009 assertNull(calibrator.getEstimatedSz());
4010 assertNull(calibrator.getEstimatedMxy());
4011 assertNull(calibrator.getEstimatedMxz());
4012 assertNull(calibrator.getEstimatedMyx());
4013 assertNull(calibrator.getEstimatedMyz());
4014 assertNull(calibrator.getEstimatedMzx());
4015 assertNull(calibrator.getEstimatedMzy());
4016 assertNull(calibrator.getEstimatedCovariance());
4017 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4018 assertNull(calibrator.getGroundTruthGravityNorm());
4019 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4020 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4021 }
4022
4023 @Test
4024 public void testConstructor38() throws WrongSizeException {
4025 final Collection<StandardDeviationBodyKinematics> measurements =
4026 Collections.emptyList();
4027
4028 final Matrix ba = generateBa();
4029 final double biasX = ba.getElementAtIndex(0);
4030 final double biasY = ba.getElementAtIndex(1);
4031 final double biasZ = ba.getElementAtIndex(2);
4032
4033 final Matrix ma = generateMaCommonAxis();
4034 final double sx = ma.getElementAt(0, 0);
4035 final double sy = ma.getElementAt(1, 1);
4036 final double sz = ma.getElementAt(2, 2);
4037
4038 final Acceleration bx = new Acceleration(biasX,
4039 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4040 final Acceleration by = new Acceleration(biasY,
4041 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4042 final Acceleration bz = new Acceleration(biasZ,
4043 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4044
4045 final KnownPositionAccelerometerCalibrator calibrator =
4046 new KnownPositionAccelerometerCalibrator(measurements,
4047 true, bx, by, bz, sx, sy, sz);
4048
4049
4050 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4051 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4052 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4053 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4054 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4055 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4056 final Acceleration bx2 = new Acceleration(0.0,
4057 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4058 calibrator.getInitialBiasXAsAcceleration(bx2);
4059 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4060 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4061 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4062 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4063 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4064 final Acceleration by2 = new Acceleration(0.0,
4065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4066 calibrator.getInitialBiasYAsAcceleration(by2);
4067 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4068 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4069 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4070 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4071 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4072 final Acceleration bz2 = new Acceleration(0.0,
4073 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4074 calibrator.getInitialBiasZAsAcceleration(bz2);
4075 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4076 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4077 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4078 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4079 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4080 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4081 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4082 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4083 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4084 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4085 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4086 final double[] bias1 = calibrator.getInitialBias();
4087 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4088 final double[] bias2 = new double[3];
4089 calibrator.getInitialBias(bias2);
4090 assertArrayEquals(bias1, bias2, 0.0);
4091 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4092 assertEquals(b1, ba);
4093 final Matrix b2 = new Matrix(3, 1);
4094 calibrator.getInitialBiasAsMatrix(b2);
4095 assertEquals(b1, b2);
4096 final Matrix ma1 = calibrator.getInitialMa();
4097 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4098 final Matrix ma2 = new Matrix(3, 3);
4099 calibrator.getInitialMa(ma2);
4100 assertEquals(ma1, ma2);
4101 assertNull(calibrator.getEcefPosition());
4102 assertNull(calibrator.getNedPosition());
4103 assertFalse(calibrator.getNedPosition(null));
4104 assertSame(calibrator.getMeasurements(), measurements);
4105 assertTrue(calibrator.isCommonAxisUsed());
4106 assertNull(calibrator.getListener());
4107 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4108 assertFalse(calibrator.isReady());
4109 assertFalse(calibrator.isRunning());
4110 assertNull(calibrator.getEstimatedBiases());
4111 assertFalse(calibrator.getEstimatedBiases(null));
4112 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4113 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4114 assertNull(calibrator.getEstimatedBiasFx());
4115 assertNull(calibrator.getEstimatedBiasFy());
4116 assertNull(calibrator.getEstimatedBiasFz());
4117 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4118 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4119 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4120 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4121 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4122 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4123 assertNull(calibrator.getEstimatedMa());
4124 assertNull(calibrator.getEstimatedSx());
4125 assertNull(calibrator.getEstimatedSy());
4126 assertNull(calibrator.getEstimatedSz());
4127 assertNull(calibrator.getEstimatedMxy());
4128 assertNull(calibrator.getEstimatedMxz());
4129 assertNull(calibrator.getEstimatedMyx());
4130 assertNull(calibrator.getEstimatedMyz());
4131 assertNull(calibrator.getEstimatedMzx());
4132 assertNull(calibrator.getEstimatedMzy());
4133 assertNull(calibrator.getEstimatedCovariance());
4134 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4135 assertNull(calibrator.getGroundTruthGravityNorm());
4136 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4137 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4138 }
4139
4140 @Test
4141 public void testConstructor39() throws WrongSizeException {
4142 final Collection<StandardDeviationBodyKinematics> measurements =
4143 Collections.emptyList();
4144
4145 final Matrix ba = generateBa();
4146 final double biasX = ba.getElementAtIndex(0);
4147 final double biasY = ba.getElementAtIndex(1);
4148 final double biasZ = ba.getElementAtIndex(2);
4149
4150 final Matrix ma = generateMaCommonAxis();
4151 final double sx = ma.getElementAt(0, 0);
4152 final double sy = ma.getElementAt(1, 1);
4153 final double sz = ma.getElementAt(2, 2);
4154
4155 final Acceleration bx = new Acceleration(biasX,
4156 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4157 final Acceleration by = new Acceleration(biasY,
4158 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4159 final Acceleration bz = new Acceleration(biasZ,
4160 AccelerationUnit.METERS_PER_SQUARED_SECOND);
4161
4162 final KnownPositionAccelerometerCalibrator calibrator =
4163 new KnownPositionAccelerometerCalibrator(measurements,
4164 true, bx, by, bz, sx, sy, sz,
4165 this);
4166
4167
4168 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4169 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4170 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4171 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4172 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4173 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4174 final Acceleration bx2 = new Acceleration(0.0,
4175 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4176 calibrator.getInitialBiasXAsAcceleration(bx2);
4177 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4178 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4179 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4180 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4181 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4182 final Acceleration by2 = new Acceleration(0.0,
4183 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4184 calibrator.getInitialBiasYAsAcceleration(by2);
4185 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4186 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4187 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4188 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4189 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4190 final Acceleration bz2 = new Acceleration(0.0,
4191 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4192 calibrator.getInitialBiasZAsAcceleration(bz2);
4193 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4194 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4195 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4196 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4197 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4198 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
4199 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
4200 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
4201 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
4202 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
4203 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
4204 final double[] bias1 = calibrator.getInitialBias();
4205 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4206 final double[] bias2 = new double[3];
4207 calibrator.getInitialBias(bias2);
4208 assertArrayEquals(bias1, bias2, 0.0);
4209 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4210 assertEquals(b1, ba);
4211 final Matrix b2 = new Matrix(3, 1);
4212 calibrator.getInitialBiasAsMatrix(b2);
4213 assertEquals(b1, b2);
4214 final Matrix ma1 = calibrator.getInitialMa();
4215 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
4216 final Matrix ma2 = new Matrix(3, 3);
4217 calibrator.getInitialMa(ma2);
4218 assertEquals(ma1, ma2);
4219 assertNull(calibrator.getEcefPosition());
4220 assertNull(calibrator.getNedPosition());
4221 assertFalse(calibrator.getNedPosition(null));
4222 assertSame(calibrator.getMeasurements(), measurements);
4223 assertTrue(calibrator.isCommonAxisUsed());
4224 assertSame(calibrator.getListener(), this);
4225 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4226 assertFalse(calibrator.isReady());
4227 assertFalse(calibrator.isRunning());
4228 assertNull(calibrator.getEstimatedBiases());
4229 assertFalse(calibrator.getEstimatedBiases(null));
4230 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4231 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4232 assertNull(calibrator.getEstimatedBiasFx());
4233 assertNull(calibrator.getEstimatedBiasFy());
4234 assertNull(calibrator.getEstimatedBiasFz());
4235 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4236 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4237 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4238 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4239 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4240 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4241 assertNull(calibrator.getEstimatedMa());
4242 assertNull(calibrator.getEstimatedSx());
4243 assertNull(calibrator.getEstimatedSy());
4244 assertNull(calibrator.getEstimatedSz());
4245 assertNull(calibrator.getEstimatedMxy());
4246 assertNull(calibrator.getEstimatedMxz());
4247 assertNull(calibrator.getEstimatedMyx());
4248 assertNull(calibrator.getEstimatedMyz());
4249 assertNull(calibrator.getEstimatedMzx());
4250 assertNull(calibrator.getEstimatedMzy());
4251 assertNull(calibrator.getEstimatedCovariance());
4252 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4253 assertNull(calibrator.getGroundTruthGravityNorm());
4254 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4255 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4256 }
4257
4258 @Test
4259 public void testConstructor40() throws WrongSizeException {
4260 final Matrix ba = generateBa();
4261 final double biasX = ba.getElementAtIndex(0);
4262 final double biasY = ba.getElementAtIndex(1);
4263 final double biasZ = ba.getElementAtIndex(2);
4264
4265 final Matrix ma = generateMaCommonAxis();
4266 final double sx = ma.getElementAt(0, 0);
4267 final double sy = ma.getElementAt(1, 1);
4268 final double sz = ma.getElementAt(2, 2);
4269 final double mxy = ma.getElementAt(0, 1);
4270 final double mxz = ma.getElementAt(0, 2);
4271 final double myx = ma.getElementAt(1, 0);
4272 final double myz = ma.getElementAt(1, 2);
4273 final double mzx = ma.getElementAt(2, 0);
4274 final double mzy = ma.getElementAt(2, 1);
4275
4276 final KnownPositionAccelerometerCalibrator calibrator =
4277 new KnownPositionAccelerometerCalibrator(biasX, biasY, biasZ,
4278 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4279
4280
4281 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4282 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4283 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4284 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4285 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4286 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4287 final Acceleration bx2 = new Acceleration(0.0,
4288 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4289 calibrator.getInitialBiasXAsAcceleration(bx2);
4290 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4291 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4292 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4293 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4294 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4295 final Acceleration by2 = new Acceleration(0.0,
4296 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4297 calibrator.getInitialBiasYAsAcceleration(by2);
4298 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4299 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4300 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4301 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4302 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4303 final Acceleration bz2 = new Acceleration(0.0,
4304 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4305 calibrator.getInitialBiasZAsAcceleration(bz2);
4306 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4307 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4308 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4309 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4310 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4311 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4312 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4313 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4314 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4315 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4316 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4317 final double[] bias1 = calibrator.getInitialBias();
4318 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4319 final double[] bias2 = new double[3];
4320 calibrator.getInitialBias(bias2);
4321 assertArrayEquals(bias1, bias2, 0.0);
4322 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4323 assertEquals(b1, ba);
4324 final Matrix b2 = new Matrix(3, 1);
4325 calibrator.getInitialBiasAsMatrix(b2);
4326 assertEquals(b1, b2);
4327 final Matrix ma1 = new Matrix(3, 3);
4328 ma1.setSubmatrix(0, 0,
4329 2, 2,
4330 new double[]{sx, myx, mzx,
4331 mxy, sy, mzy,
4332 mxz, myz, sz});
4333 assertEquals(calibrator.getInitialMa(), ma1);
4334 final Matrix ma2 = new Matrix(3, 3);
4335 calibrator.getInitialMa(ma2);
4336 assertEquals(ma1, ma2);
4337 assertNull(calibrator.getEcefPosition());
4338 assertNull(calibrator.getNedPosition());
4339 assertFalse(calibrator.getNedPosition(null));
4340 assertNull(calibrator.getMeasurements());
4341 assertFalse(calibrator.isCommonAxisUsed());
4342 assertNull(calibrator.getListener());
4343 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4344 assertFalse(calibrator.isReady());
4345 assertFalse(calibrator.isRunning());
4346 assertNull(calibrator.getEstimatedBiases());
4347 assertFalse(calibrator.getEstimatedBiases(null));
4348 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4349 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4350 assertNull(calibrator.getEstimatedBiasFx());
4351 assertNull(calibrator.getEstimatedBiasFy());
4352 assertNull(calibrator.getEstimatedBiasFz());
4353 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4354 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4355 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4356 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4357 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4358 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4359 assertNull(calibrator.getEstimatedMa());
4360 assertNull(calibrator.getEstimatedSx());
4361 assertNull(calibrator.getEstimatedSy());
4362 assertNull(calibrator.getEstimatedSz());
4363 assertNull(calibrator.getEstimatedMxy());
4364 assertNull(calibrator.getEstimatedMxz());
4365 assertNull(calibrator.getEstimatedMyx());
4366 assertNull(calibrator.getEstimatedMyz());
4367 assertNull(calibrator.getEstimatedMzx());
4368 assertNull(calibrator.getEstimatedMzy());
4369 assertNull(calibrator.getEstimatedCovariance());
4370 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4371 assertNull(calibrator.getGroundTruthGravityNorm());
4372 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4373 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4374 }
4375
4376 @Test
4377 public void testConstructor41() throws WrongSizeException {
4378 final Collection<StandardDeviationBodyKinematics> measurements =
4379 Collections.emptyList();
4380
4381 final Matrix ba = generateBa();
4382 final double biasX = ba.getElementAtIndex(0);
4383 final double biasY = ba.getElementAtIndex(1);
4384 final double biasZ = ba.getElementAtIndex(2);
4385
4386 final Matrix ma = generateMaCommonAxis();
4387 final double sx = ma.getElementAt(0, 0);
4388 final double sy = ma.getElementAt(1, 1);
4389 final double sz = ma.getElementAt(2, 2);
4390 final double mxy = ma.getElementAt(0, 1);
4391 final double mxz = ma.getElementAt(0, 2);
4392 final double myx = ma.getElementAt(1, 0);
4393 final double myz = ma.getElementAt(1, 2);
4394 final double mzx = ma.getElementAt(2, 0);
4395 final double mzy = ma.getElementAt(2, 1);
4396
4397 final KnownPositionAccelerometerCalibrator calibrator =
4398 new KnownPositionAccelerometerCalibrator(measurements,
4399 biasX, biasY, biasZ, sx, sy, sz,
4400 mxy, mxz, myx, myz, mzx, mzy);
4401
4402
4403 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4404 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4405 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4406 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4407 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4408 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4409 final Acceleration bx2 = new Acceleration(0.0,
4410 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4411 calibrator.getInitialBiasXAsAcceleration(bx2);
4412 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4413 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4414 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4415 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4416 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4417 final Acceleration by2 = new Acceleration(0.0,
4418 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4419 calibrator.getInitialBiasYAsAcceleration(by2);
4420 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4421 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4422 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4423 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4424 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4425 final Acceleration bz2 = new Acceleration(0.0,
4426 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4427 calibrator.getInitialBiasZAsAcceleration(bz2);
4428 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4429 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4430 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4431 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4432 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4433 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4434 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4435 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4436 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4437 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4438 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4439 final double[] bias1 = calibrator.getInitialBias();
4440 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4441 final double[] bias2 = new double[3];
4442 calibrator.getInitialBias(bias2);
4443 assertArrayEquals(bias1, bias2, 0.0);
4444 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4445 assertEquals(b1, ba);
4446 final Matrix b2 = new Matrix(3, 1);
4447 calibrator.getInitialBiasAsMatrix(b2);
4448 assertEquals(b1, b2);
4449 final Matrix ma1 = new Matrix(3, 3);
4450 ma1.setSubmatrix(0, 0,
4451 2, 2,
4452 new double[]{sx, myx, mzx,
4453 mxy, sy, mzy,
4454 mxz, myz, sz});
4455 assertEquals(calibrator.getInitialMa(), ma1);
4456 final Matrix ma2 = new Matrix(3, 3);
4457 calibrator.getInitialMa(ma2);
4458 assertEquals(ma1, ma2);
4459 assertNull(calibrator.getEcefPosition());
4460 assertNull(calibrator.getNedPosition());
4461 assertFalse(calibrator.getNedPosition(null));
4462 assertSame(calibrator.getMeasurements(), measurements);
4463 assertFalse(calibrator.isCommonAxisUsed());
4464 assertNull(calibrator.getListener());
4465 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4466 assertFalse(calibrator.isReady());
4467 assertFalse(calibrator.isRunning());
4468 assertNull(calibrator.getEstimatedBiases());
4469 assertFalse(calibrator.getEstimatedBiases(null));
4470 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4471 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4472 assertNull(calibrator.getEstimatedBiasFx());
4473 assertNull(calibrator.getEstimatedBiasFy());
4474 assertNull(calibrator.getEstimatedBiasFz());
4475 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4476 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4477 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4478 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4479 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4480 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4481 assertNull(calibrator.getEstimatedMa());
4482 assertNull(calibrator.getEstimatedSx());
4483 assertNull(calibrator.getEstimatedSy());
4484 assertNull(calibrator.getEstimatedSz());
4485 assertNull(calibrator.getEstimatedMxy());
4486 assertNull(calibrator.getEstimatedMxz());
4487 assertNull(calibrator.getEstimatedMyx());
4488 assertNull(calibrator.getEstimatedMyz());
4489 assertNull(calibrator.getEstimatedMzx());
4490 assertNull(calibrator.getEstimatedMzy());
4491 assertNull(calibrator.getEstimatedCovariance());
4492 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4493 assertNull(calibrator.getGroundTruthGravityNorm());
4494 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4495 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4496 }
4497
4498 @Test
4499 public void testConstructor42() throws WrongSizeException {
4500 final Collection<StandardDeviationBodyKinematics> measurements =
4501 Collections.emptyList();
4502
4503 final Matrix ba = generateBa();
4504 final double biasX = ba.getElementAtIndex(0);
4505 final double biasY = ba.getElementAtIndex(1);
4506 final double biasZ = ba.getElementAtIndex(2);
4507
4508 final Matrix ma = generateMaCommonAxis();
4509 final double sx = ma.getElementAt(0, 0);
4510 final double sy = ma.getElementAt(1, 1);
4511 final double sz = ma.getElementAt(2, 2);
4512 final double mxy = ma.getElementAt(0, 1);
4513 final double mxz = ma.getElementAt(0, 2);
4514 final double myx = ma.getElementAt(1, 0);
4515 final double myz = ma.getElementAt(1, 2);
4516 final double mzx = ma.getElementAt(2, 0);
4517 final double mzy = ma.getElementAt(2, 1);
4518
4519 final KnownPositionAccelerometerCalibrator calibrator =
4520 new KnownPositionAccelerometerCalibrator(measurements,
4521 biasX, biasY, biasZ, sx, sy, sz,
4522 mxy, mxz, myx, myz, mzx, mzy, this);
4523
4524
4525 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4526 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4527 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4528 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4529 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4530 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4531 final Acceleration bx2 = new Acceleration(0.0,
4532 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4533 calibrator.getInitialBiasXAsAcceleration(bx2);
4534 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4535 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4536 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4537 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4538 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4539 final Acceleration by2 = new Acceleration(0.0,
4540 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4541 calibrator.getInitialBiasYAsAcceleration(by2);
4542 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4543 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4544 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4545 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4546 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4547 final Acceleration bz2 = new Acceleration(0.0,
4548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4549 calibrator.getInitialBiasZAsAcceleration(bz2);
4550 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4551 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4552 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4553 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4554 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4555 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4556 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4557 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4558 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4559 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4560 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4561 final double[] bias1 = calibrator.getInitialBias();
4562 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4563 final double[] bias2 = new double[3];
4564 calibrator.getInitialBias(bias2);
4565 assertArrayEquals(bias1, bias2, 0.0);
4566 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4567 assertEquals(b1, ba);
4568 final Matrix b2 = new Matrix(3, 1);
4569 calibrator.getInitialBiasAsMatrix(b2);
4570 assertEquals(b1, b2);
4571 final Matrix ma1 = new Matrix(3, 3);
4572 ma1.setSubmatrix(0, 0,
4573 2, 2,
4574 new double[]{sx, myx, mzx,
4575 mxy, sy, mzy,
4576 mxz, myz, sz});
4577 assertEquals(calibrator.getInitialMa(), ma1);
4578 final Matrix ma2 = new Matrix(3, 3);
4579 calibrator.getInitialMa(ma2);
4580 assertEquals(ma1, ma2);
4581 assertNull(calibrator.getEcefPosition());
4582 assertNull(calibrator.getNedPosition());
4583 assertFalse(calibrator.getNedPosition(null));
4584 assertSame(calibrator.getMeasurements(), measurements);
4585 assertFalse(calibrator.isCommonAxisUsed());
4586 assertSame(calibrator.getListener(), this);
4587 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
4588 assertFalse(calibrator.isReady());
4589 assertFalse(calibrator.isRunning());
4590 assertNull(calibrator.getEstimatedBiases());
4591 assertFalse(calibrator.getEstimatedBiases(null));
4592 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4593 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4594 assertNull(calibrator.getEstimatedBiasFx());
4595 assertNull(calibrator.getEstimatedBiasFy());
4596 assertNull(calibrator.getEstimatedBiasFz());
4597 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4598 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4599 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4600 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4601 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4602 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4603 assertNull(calibrator.getEstimatedMa());
4604 assertNull(calibrator.getEstimatedSx());
4605 assertNull(calibrator.getEstimatedSy());
4606 assertNull(calibrator.getEstimatedSz());
4607 assertNull(calibrator.getEstimatedMxy());
4608 assertNull(calibrator.getEstimatedMxz());
4609 assertNull(calibrator.getEstimatedMyx());
4610 assertNull(calibrator.getEstimatedMyz());
4611 assertNull(calibrator.getEstimatedMzx());
4612 assertNull(calibrator.getEstimatedMzy());
4613 assertNull(calibrator.getEstimatedCovariance());
4614 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4615 assertNull(calibrator.getGroundTruthGravityNorm());
4616 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4617 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4618 }
4619
4620 @Test
4621 public void testConstructor43() throws WrongSizeException {
4622 final Matrix ba = generateBa();
4623 final double biasX = ba.getElementAtIndex(0);
4624 final double biasY = ba.getElementAtIndex(1);
4625 final double biasZ = ba.getElementAtIndex(2);
4626
4627 final Matrix ma = generateMaCommonAxis();
4628 final double sx = ma.getElementAt(0, 0);
4629 final double sy = ma.getElementAt(1, 1);
4630 final double sz = ma.getElementAt(2, 2);
4631 final double mxy = ma.getElementAt(0, 1);
4632 final double mxz = ma.getElementAt(0, 2);
4633 final double myx = ma.getElementAt(1, 0);
4634 final double myz = ma.getElementAt(1, 2);
4635 final double mzx = ma.getElementAt(2, 0);
4636 final double mzy = ma.getElementAt(2, 1);
4637
4638 final KnownPositionAccelerometerCalibrator calibrator =
4639 new KnownPositionAccelerometerCalibrator(true,
4640 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4641 myz, mzx, mzy);
4642
4643
4644 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4645 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4646 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4647 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4648 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4649 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4650 final Acceleration bx2 = new Acceleration(0.0,
4651 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4652 calibrator.getInitialBiasXAsAcceleration(bx2);
4653 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4654 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4655 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4656 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4657 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4658 final Acceleration by2 = new Acceleration(0.0,
4659 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4660 calibrator.getInitialBiasYAsAcceleration(by2);
4661 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4662 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4663 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4664 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4665 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4666 final Acceleration bz2 = new Acceleration(0.0,
4667 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4668 calibrator.getInitialBiasZAsAcceleration(bz2);
4669 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4670 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4671 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4672 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4673 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4674 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4675 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4676 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4677 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4678 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4679 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4680 final double[] bias1 = calibrator.getInitialBias();
4681 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4682 final double[] bias2 = new double[3];
4683 calibrator.getInitialBias(bias2);
4684 assertArrayEquals(bias1, bias2, 0.0);
4685 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4686 assertEquals(b1, ba);
4687 final Matrix b2 = new Matrix(3, 1);
4688 calibrator.getInitialBiasAsMatrix(b2);
4689 assertEquals(b1, b2);
4690 final Matrix ma1 = new Matrix(3, 3);
4691 ma1.setSubmatrix(0, 0,
4692 2, 2,
4693 new double[]{sx, myx, mzx,
4694 mxy, sy, mzy,
4695 mxz, myz, sz});
4696 assertEquals(calibrator.getInitialMa(), ma1);
4697 final Matrix ma2 = new Matrix(3, 3);
4698 calibrator.getInitialMa(ma2);
4699 assertEquals(ma1, ma2);
4700 assertNull(calibrator.getEcefPosition());
4701 assertNull(calibrator.getNedPosition());
4702 assertFalse(calibrator.getNedPosition(null));
4703 assertNull(calibrator.getMeasurements());
4704 assertTrue(calibrator.isCommonAxisUsed());
4705 assertNull(calibrator.getListener());
4706 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4707 assertFalse(calibrator.isReady());
4708 assertFalse(calibrator.isRunning());
4709 assertNull(calibrator.getEstimatedBiases());
4710 assertFalse(calibrator.getEstimatedBiases(null));
4711 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4712 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4713 assertNull(calibrator.getEstimatedBiasFx());
4714 assertNull(calibrator.getEstimatedBiasFy());
4715 assertNull(calibrator.getEstimatedBiasFz());
4716 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4717 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4718 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4719 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4720 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4721 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4722 assertNull(calibrator.getEstimatedMa());
4723 assertNull(calibrator.getEstimatedSx());
4724 assertNull(calibrator.getEstimatedSy());
4725 assertNull(calibrator.getEstimatedSz());
4726 assertNull(calibrator.getEstimatedMxy());
4727 assertNull(calibrator.getEstimatedMxz());
4728 assertNull(calibrator.getEstimatedMyx());
4729 assertNull(calibrator.getEstimatedMyz());
4730 assertNull(calibrator.getEstimatedMzx());
4731 assertNull(calibrator.getEstimatedMzy());
4732 assertNull(calibrator.getEstimatedCovariance());
4733 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4734 assertNull(calibrator.getGroundTruthGravityNorm());
4735 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4736 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4737 }
4738
4739 @Test
4740 public void testConstructor44() throws WrongSizeException {
4741 final Matrix ba = generateBa();
4742 final double biasX = ba.getElementAtIndex(0);
4743 final double biasY = ba.getElementAtIndex(1);
4744 final double biasZ = ba.getElementAtIndex(2);
4745
4746 final Matrix ma = generateMaCommonAxis();
4747 final double sx = ma.getElementAt(0, 0);
4748 final double sy = ma.getElementAt(1, 1);
4749 final double sz = ma.getElementAt(2, 2);
4750 final double mxy = ma.getElementAt(0, 1);
4751 final double mxz = ma.getElementAt(0, 2);
4752 final double myx = ma.getElementAt(1, 0);
4753 final double myz = ma.getElementAt(1, 2);
4754 final double mzx = ma.getElementAt(2, 0);
4755 final double mzy = ma.getElementAt(2, 1);
4756
4757 final KnownPositionAccelerometerCalibrator calibrator =
4758 new KnownPositionAccelerometerCalibrator(true,
4759 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz, myx,
4760 myz, mzx, mzy, this);
4761
4762
4763 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4764 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4765 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4766 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4767 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4768 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4769 final Acceleration bx2 = new Acceleration(0.0,
4770 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4771 calibrator.getInitialBiasXAsAcceleration(bx2);
4772 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4773 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4774 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4775 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4776 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4777 final Acceleration by2 = new Acceleration(0.0,
4778 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4779 calibrator.getInitialBiasYAsAcceleration(by2);
4780 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4781 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4782 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4783 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4784 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4785 final Acceleration bz2 = new Acceleration(0.0,
4786 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4787 calibrator.getInitialBiasZAsAcceleration(bz2);
4788 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4789 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4790 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4791 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4792 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4793 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4794 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4795 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4796 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4797 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4798 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4799 final double[] bias1 = calibrator.getInitialBias();
4800 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4801 final double[] bias2 = new double[3];
4802 calibrator.getInitialBias(bias2);
4803 assertArrayEquals(bias1, bias2, 0.0);
4804 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4805 assertEquals(b1, ba);
4806 final Matrix b2 = new Matrix(3, 1);
4807 calibrator.getInitialBiasAsMatrix(b2);
4808 assertEquals(b1, b2);
4809 final Matrix ma1 = new Matrix(3, 3);
4810 ma1.setSubmatrix(0, 0,
4811 2, 2,
4812 new double[]{sx, myx, mzx,
4813 mxy, sy, mzy,
4814 mxz, myz, sz});
4815 assertEquals(calibrator.getInitialMa(), ma1);
4816 final Matrix ma2 = new Matrix(3, 3);
4817 calibrator.getInitialMa(ma2);
4818 assertEquals(ma1, ma2);
4819 assertNull(calibrator.getEcefPosition());
4820 assertNull(calibrator.getNedPosition());
4821 assertFalse(calibrator.getNedPosition(null));
4822 assertNull(calibrator.getMeasurements());
4823 assertTrue(calibrator.isCommonAxisUsed());
4824 assertSame(calibrator.getListener(), this);
4825 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4826 assertFalse(calibrator.isReady());
4827 assertFalse(calibrator.isRunning());
4828 assertNull(calibrator.getEstimatedBiases());
4829 assertFalse(calibrator.getEstimatedBiases(null));
4830 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4831 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4832 assertNull(calibrator.getEstimatedBiasFx());
4833 assertNull(calibrator.getEstimatedBiasFy());
4834 assertNull(calibrator.getEstimatedBiasFz());
4835 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4836 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4837 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4838 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4839 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4840 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4841 assertNull(calibrator.getEstimatedMa());
4842 assertNull(calibrator.getEstimatedSx());
4843 assertNull(calibrator.getEstimatedSy());
4844 assertNull(calibrator.getEstimatedSz());
4845 assertNull(calibrator.getEstimatedMxy());
4846 assertNull(calibrator.getEstimatedMxz());
4847 assertNull(calibrator.getEstimatedMyx());
4848 assertNull(calibrator.getEstimatedMyz());
4849 assertNull(calibrator.getEstimatedMzx());
4850 assertNull(calibrator.getEstimatedMzy());
4851 assertNull(calibrator.getEstimatedCovariance());
4852 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4853 assertNull(calibrator.getGroundTruthGravityNorm());
4854 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4855 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4856 }
4857
4858 @Test
4859 public void testConstructor45() throws WrongSizeException {
4860 final Collection<StandardDeviationBodyKinematics> measurements =
4861 Collections.emptyList();
4862
4863 final Matrix ba = generateBa();
4864 final double biasX = ba.getElementAtIndex(0);
4865 final double biasY = ba.getElementAtIndex(1);
4866 final double biasZ = ba.getElementAtIndex(2);
4867
4868 final Matrix ma = generateMaCommonAxis();
4869 final double sx = ma.getElementAt(0, 0);
4870 final double sy = ma.getElementAt(1, 1);
4871 final double sz = ma.getElementAt(2, 2);
4872 final double mxy = ma.getElementAt(0, 1);
4873 final double mxz = ma.getElementAt(0, 2);
4874 final double myx = ma.getElementAt(1, 0);
4875 final double myz = ma.getElementAt(1, 2);
4876 final double mzx = ma.getElementAt(2, 0);
4877 final double mzy = ma.getElementAt(2, 1);
4878
4879 final KnownPositionAccelerometerCalibrator calibrator =
4880 new KnownPositionAccelerometerCalibrator(measurements,
4881 true, biasX, biasY, biasZ,
4882 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
4883
4884
4885 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
4886 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
4887 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
4888 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
4889 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
4890 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4891 final Acceleration bx2 = new Acceleration(0.0,
4892 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4893 calibrator.getInitialBiasXAsAcceleration(bx2);
4894 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
4895 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4896 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
4897 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
4898 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4899 final Acceleration by2 = new Acceleration(0.0,
4900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4901 calibrator.getInitialBiasYAsAcceleration(by2);
4902 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
4903 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4904 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
4905 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
4906 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4907 final Acceleration bz2 = new Acceleration(0.0,
4908 AccelerationUnit.FEET_PER_SQUARED_SECOND);
4909 calibrator.getInitialBiasZAsAcceleration(bz2);
4910 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
4911 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
4912 assertEquals(calibrator.getInitialSx(), sx, 0.0);
4913 assertEquals(calibrator.getInitialSy(), sy, 0.0);
4914 assertEquals(calibrator.getInitialSz(), sz, 0.0);
4915 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
4916 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
4917 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
4918 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
4919 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
4920 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
4921 final double[] bias1 = calibrator.getInitialBias();
4922 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
4923 final double[] bias2 = new double[3];
4924 calibrator.getInitialBias(bias2);
4925 assertArrayEquals(bias1, bias2, 0.0);
4926 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
4927 assertEquals(b1, ba);
4928 final Matrix b2 = new Matrix(3, 1);
4929 calibrator.getInitialBiasAsMatrix(b2);
4930 assertEquals(b1, b2);
4931 final Matrix ma1 = new Matrix(3, 3);
4932 ma1.setSubmatrix(0, 0,
4933 2, 2,
4934 new double[]{sx, myx, mzx,
4935 mxy, sy, mzy,
4936 mxz, myz, sz});
4937 assertEquals(calibrator.getInitialMa(), ma1);
4938 final Matrix ma2 = new Matrix(3, 3);
4939 calibrator.getInitialMa(ma2);
4940 assertEquals(ma1, ma2);
4941 assertNull(calibrator.getEcefPosition());
4942 assertNull(calibrator.getNedPosition());
4943 assertFalse(calibrator.getNedPosition(null));
4944 assertSame(calibrator.getMeasurements(), measurements);
4945 assertTrue(calibrator.isCommonAxisUsed());
4946 assertNull(calibrator.getListener());
4947 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
4948 assertFalse(calibrator.isReady());
4949 assertFalse(calibrator.isRunning());
4950 assertNull(calibrator.getEstimatedBiases());
4951 assertFalse(calibrator.getEstimatedBiases(null));
4952 assertNull(calibrator.getEstimatedBiasesAsMatrix());
4953 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
4954 assertNull(calibrator.getEstimatedBiasFx());
4955 assertNull(calibrator.getEstimatedBiasFy());
4956 assertNull(calibrator.getEstimatedBiasFz());
4957 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
4958 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
4959 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
4960 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
4961 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
4962 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
4963 assertNull(calibrator.getEstimatedMa());
4964 assertNull(calibrator.getEstimatedSx());
4965 assertNull(calibrator.getEstimatedSy());
4966 assertNull(calibrator.getEstimatedSz());
4967 assertNull(calibrator.getEstimatedMxy());
4968 assertNull(calibrator.getEstimatedMxz());
4969 assertNull(calibrator.getEstimatedMyx());
4970 assertNull(calibrator.getEstimatedMyz());
4971 assertNull(calibrator.getEstimatedMzx());
4972 assertNull(calibrator.getEstimatedMzy());
4973 assertNull(calibrator.getEstimatedCovariance());
4974 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
4975 assertNull(calibrator.getGroundTruthGravityNorm());
4976 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
4977 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
4978 }
4979
4980 @Test
4981 public void testConstructor46() throws WrongSizeException {
4982 final Collection<StandardDeviationBodyKinematics> measurements =
4983 Collections.emptyList();
4984
4985 final Matrix ba = generateBa();
4986 final double biasX = ba.getElementAtIndex(0);
4987 final double biasY = ba.getElementAtIndex(1);
4988 final double biasZ = ba.getElementAtIndex(2);
4989
4990 final Matrix ma = generateMaCommonAxis();
4991 final double sx = ma.getElementAt(0, 0);
4992 final double sy = ma.getElementAt(1, 1);
4993 final double sz = ma.getElementAt(2, 2);
4994 final double mxy = ma.getElementAt(0, 1);
4995 final double mxz = ma.getElementAt(0, 2);
4996 final double myx = ma.getElementAt(1, 0);
4997 final double myz = ma.getElementAt(1, 2);
4998 final double mzx = ma.getElementAt(2, 0);
4999 final double mzy = ma.getElementAt(2, 1);
5000
5001 final KnownPositionAccelerometerCalibrator calibrator =
5002 new KnownPositionAccelerometerCalibrator(measurements,
5003 true, biasX, biasY, biasZ,
5004 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5005
5006
5007 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5008 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5009 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5010 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5011 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5012 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5013 final Acceleration bx2 = new Acceleration(0.0,
5014 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5015 calibrator.getInitialBiasXAsAcceleration(bx2);
5016 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5017 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5018 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5019 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5020 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5021 final Acceleration by2 = new Acceleration(0.0,
5022 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5023 calibrator.getInitialBiasYAsAcceleration(by2);
5024 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5025 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5026 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5027 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5028 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5029 final Acceleration bz2 = new Acceleration(0.0,
5030 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5031 calibrator.getInitialBiasZAsAcceleration(bz2);
5032 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5033 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5034 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5035 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5036 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5037 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5038 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5039 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5040 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5041 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5042 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5043 final double[] bias1 = calibrator.getInitialBias();
5044 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5045 final double[] bias2 = new double[3];
5046 calibrator.getInitialBias(bias2);
5047 assertArrayEquals(bias1, bias2, 0.0);
5048 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5049 assertEquals(b1, ba);
5050 final Matrix b2 = new Matrix(3, 1);
5051 calibrator.getInitialBiasAsMatrix(b2);
5052 assertEquals(b1, b2);
5053 final Matrix ma1 = new Matrix(3, 3);
5054 ma1.setSubmatrix(0, 0,
5055 2, 2,
5056 new double[]{sx, myx, mzx,
5057 mxy, sy, mzy,
5058 mxz, myz, sz});
5059 assertEquals(calibrator.getInitialMa(), ma1);
5060 final Matrix ma2 = new Matrix(3, 3);
5061 calibrator.getInitialMa(ma2);
5062 assertEquals(ma1, ma2);
5063 assertNull(calibrator.getEcefPosition());
5064 assertNull(calibrator.getNedPosition());
5065 assertFalse(calibrator.getNedPosition(null));
5066 assertSame(calibrator.getMeasurements(), measurements);
5067 assertTrue(calibrator.isCommonAxisUsed());
5068 assertSame(calibrator.getListener(), this);
5069 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5070 assertFalse(calibrator.isReady());
5071 assertFalse(calibrator.isRunning());
5072 assertNull(calibrator.getEstimatedBiases());
5073 assertFalse(calibrator.getEstimatedBiases(null));
5074 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5075 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5076 assertNull(calibrator.getEstimatedBiasFx());
5077 assertNull(calibrator.getEstimatedBiasFy());
5078 assertNull(calibrator.getEstimatedBiasFz());
5079 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5080 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5081 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5082 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5083 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5084 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5085 assertNull(calibrator.getEstimatedMa());
5086 assertNull(calibrator.getEstimatedSx());
5087 assertNull(calibrator.getEstimatedSy());
5088 assertNull(calibrator.getEstimatedSz());
5089 assertNull(calibrator.getEstimatedMxy());
5090 assertNull(calibrator.getEstimatedMxz());
5091 assertNull(calibrator.getEstimatedMyx());
5092 assertNull(calibrator.getEstimatedMyz());
5093 assertNull(calibrator.getEstimatedMzx());
5094 assertNull(calibrator.getEstimatedMzy());
5095 assertNull(calibrator.getEstimatedCovariance());
5096 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5097 assertNull(calibrator.getGroundTruthGravityNorm());
5098 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5099 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5100 }
5101
5102 @Test
5103 public void testConstructor47() throws WrongSizeException {
5104 final Matrix ba = generateBa();
5105 final double biasX = ba.getElementAtIndex(0);
5106 final double biasY = ba.getElementAtIndex(1);
5107 final double biasZ = ba.getElementAtIndex(2);
5108
5109 final Matrix ma = generateMaCommonAxis();
5110 final double sx = ma.getElementAt(0, 0);
5111 final double sy = ma.getElementAt(1, 1);
5112 final double sz = ma.getElementAt(2, 2);
5113 final double mxy = ma.getElementAt(0, 1);
5114 final double mxz = ma.getElementAt(0, 2);
5115 final double myx = ma.getElementAt(1, 0);
5116 final double myz = ma.getElementAt(1, 2);
5117 final double mzx = ma.getElementAt(2, 0);
5118 final double mzy = ma.getElementAt(2, 1);
5119
5120 final Acceleration bx = new Acceleration(biasX,
5121 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5122 final Acceleration by = new Acceleration(biasY,
5123 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5124 final Acceleration bz = new Acceleration(biasZ,
5125 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5126
5127 final KnownPositionAccelerometerCalibrator calibrator =
5128 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5129 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5130
5131
5132 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5133 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5134 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5135 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5136 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5137 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5138 final Acceleration bx2 = new Acceleration(0.0,
5139 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5140 calibrator.getInitialBiasXAsAcceleration(bx2);
5141 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5142 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5143 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5144 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5145 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5146 final Acceleration by2 = new Acceleration(0.0,
5147 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5148 calibrator.getInitialBiasYAsAcceleration(by2);
5149 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5150 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5151 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5152 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5153 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5154 final Acceleration bz2 = new Acceleration(0.0,
5155 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5156 calibrator.getInitialBiasZAsAcceleration(bz2);
5157 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5158 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5159 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5160 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5161 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5162 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5163 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5164 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5165 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5166 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5167 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5168 final double[] bias1 = calibrator.getInitialBias();
5169 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5170 final double[] bias2 = new double[3];
5171 calibrator.getInitialBias(bias2);
5172 assertArrayEquals(bias1, bias2, 0.0);
5173 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5174 assertEquals(b1, ba);
5175 final Matrix b2 = new Matrix(3, 1);
5176 calibrator.getInitialBiasAsMatrix(b2);
5177 assertEquals(b1, b2);
5178 final Matrix ma1 = new Matrix(3, 3);
5179 ma1.setSubmatrix(0, 0,
5180 2, 2,
5181 new double[]{sx, myx, mzx,
5182 mxy, sy, mzy,
5183 mxz, myz, sz});
5184 assertEquals(calibrator.getInitialMa(), ma1);
5185 final Matrix ma2 = new Matrix(3, 3);
5186 calibrator.getInitialMa(ma2);
5187 assertEquals(ma1, ma2);
5188 assertNull(calibrator.getEcefPosition());
5189 assertNull(calibrator.getNedPosition());
5190 assertFalse(calibrator.getNedPosition(null));
5191 assertNull(calibrator.getMeasurements());
5192 assertFalse(calibrator.isCommonAxisUsed());
5193 assertNull(calibrator.getListener());
5194 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5195 assertFalse(calibrator.isReady());
5196 assertFalse(calibrator.isRunning());
5197 assertNull(calibrator.getEstimatedBiases());
5198 assertFalse(calibrator.getEstimatedBiases(null));
5199 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5200 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5201 assertNull(calibrator.getEstimatedBiasFx());
5202 assertNull(calibrator.getEstimatedBiasFy());
5203 assertNull(calibrator.getEstimatedBiasFz());
5204 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5205 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5206 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5207 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5208 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5209 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5210 assertNull(calibrator.getEstimatedMa());
5211 assertNull(calibrator.getEstimatedSx());
5212 assertNull(calibrator.getEstimatedSy());
5213 assertNull(calibrator.getEstimatedSz());
5214 assertNull(calibrator.getEstimatedMxy());
5215 assertNull(calibrator.getEstimatedMxz());
5216 assertNull(calibrator.getEstimatedMyx());
5217 assertNull(calibrator.getEstimatedMyz());
5218 assertNull(calibrator.getEstimatedMzx());
5219 assertNull(calibrator.getEstimatedMzy());
5220 assertNull(calibrator.getEstimatedCovariance());
5221 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5222 assertNull(calibrator.getGroundTruthGravityNorm());
5223 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5224 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5225 }
5226
5227 @Test
5228 public void testConstructor48() throws WrongSizeException {
5229 final Matrix ba = generateBa();
5230 final double biasX = ba.getElementAtIndex(0);
5231 final double biasY = ba.getElementAtIndex(1);
5232 final double biasZ = ba.getElementAtIndex(2);
5233
5234 final Matrix ma = generateMaCommonAxis();
5235 final double sx = ma.getElementAt(0, 0);
5236 final double sy = ma.getElementAt(1, 1);
5237 final double sz = ma.getElementAt(2, 2);
5238 final double mxy = ma.getElementAt(0, 1);
5239 final double mxz = ma.getElementAt(0, 2);
5240 final double myx = ma.getElementAt(1, 0);
5241 final double myz = ma.getElementAt(1, 2);
5242 final double mzx = ma.getElementAt(2, 0);
5243 final double mzy = ma.getElementAt(2, 1);
5244
5245 final Acceleration bx = new Acceleration(biasX,
5246 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5247 final Acceleration by = new Acceleration(biasY,
5248 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5249 final Acceleration bz = new Acceleration(biasZ,
5250 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5251
5252 final KnownPositionAccelerometerCalibrator calibrator =
5253 new KnownPositionAccelerometerCalibrator(bx, by, bz,
5254 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5255
5256
5257 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5258 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5259 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5260 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5261 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5262 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5263 final Acceleration bx2 = new Acceleration(0.0,
5264 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5265 calibrator.getInitialBiasXAsAcceleration(bx2);
5266 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5267 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5268 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5269 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5270 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5271 final Acceleration by2 = new Acceleration(0.0,
5272 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5273 calibrator.getInitialBiasYAsAcceleration(by2);
5274 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5275 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5276 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5277 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5278 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5279 final Acceleration bz2 = new Acceleration(0.0,
5280 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5281 calibrator.getInitialBiasZAsAcceleration(bz2);
5282 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5283 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5284 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5285 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5286 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5287 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5288 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5289 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5290 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5291 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5292 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5293 final double[] bias1 = calibrator.getInitialBias();
5294 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5295 final double[] bias2 = new double[3];
5296 calibrator.getInitialBias(bias2);
5297 assertArrayEquals(bias1, bias2, 0.0);
5298 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5299 assertEquals(b1, ba);
5300 final Matrix b2 = new Matrix(3, 1);
5301 calibrator.getInitialBiasAsMatrix(b2);
5302 assertEquals(b1, b2);
5303 final Matrix ma1 = new Matrix(3, 3);
5304 ma1.setSubmatrix(0, 0,
5305 2, 2,
5306 new double[]{sx, myx, mzx,
5307 mxy, sy, mzy,
5308 mxz, myz, sz});
5309 assertEquals(calibrator.getInitialMa(), ma1);
5310 final Matrix ma2 = new Matrix(3, 3);
5311 calibrator.getInitialMa(ma2);
5312 assertEquals(ma1, ma2);
5313 assertNull(calibrator.getEcefPosition());
5314 assertNull(calibrator.getNedPosition());
5315 assertFalse(calibrator.getNedPosition(null));
5316 assertNull(calibrator.getMeasurements());
5317 assertFalse(calibrator.isCommonAxisUsed());
5318 assertSame(calibrator.getListener(), this);
5319 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5320 assertFalse(calibrator.isReady());
5321 assertFalse(calibrator.isRunning());
5322 assertNull(calibrator.getEstimatedBiases());
5323 assertFalse(calibrator.getEstimatedBiases(null));
5324 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5325 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5326 assertNull(calibrator.getEstimatedBiasFx());
5327 assertNull(calibrator.getEstimatedBiasFy());
5328 assertNull(calibrator.getEstimatedBiasFz());
5329 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5330 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5331 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5332 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5333 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5334 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5335 assertNull(calibrator.getEstimatedMa());
5336 assertNull(calibrator.getEstimatedSx());
5337 assertNull(calibrator.getEstimatedSy());
5338 assertNull(calibrator.getEstimatedSz());
5339 assertNull(calibrator.getEstimatedMxy());
5340 assertNull(calibrator.getEstimatedMxz());
5341 assertNull(calibrator.getEstimatedMyx());
5342 assertNull(calibrator.getEstimatedMyz());
5343 assertNull(calibrator.getEstimatedMzx());
5344 assertNull(calibrator.getEstimatedMzy());
5345 assertNull(calibrator.getEstimatedCovariance());
5346 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5347 assertNull(calibrator.getGroundTruthGravityNorm());
5348 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5349 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5350 }
5351
5352 @Test
5353 public void testConstructor49() throws WrongSizeException {
5354
5355 final Collection<StandardDeviationBodyKinematics> measurements =
5356 Collections.emptyList();
5357
5358 final Matrix ba = generateBa();
5359 final double biasX = ba.getElementAtIndex(0);
5360 final double biasY = ba.getElementAtIndex(1);
5361 final double biasZ = ba.getElementAtIndex(2);
5362
5363 final Matrix ma = generateMaCommonAxis();
5364 final double sx = ma.getElementAt(0, 0);
5365 final double sy = ma.getElementAt(1, 1);
5366 final double sz = ma.getElementAt(2, 2);
5367 final double mxy = ma.getElementAt(0, 1);
5368 final double mxz = ma.getElementAt(0, 2);
5369 final double myx = ma.getElementAt(1, 0);
5370 final double myz = ma.getElementAt(1, 2);
5371 final double mzx = ma.getElementAt(2, 0);
5372 final double mzy = ma.getElementAt(2, 1);
5373
5374 final Acceleration bx = new Acceleration(biasX,
5375 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5376 final Acceleration by = new Acceleration(biasY,
5377 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5378 final Acceleration bz = new Acceleration(biasZ,
5379 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5380
5381 final KnownPositionAccelerometerCalibrator calibrator =
5382 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5383 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5384
5385
5386 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5387 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5388 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5389 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5390 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5391 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5392 final Acceleration bx2 = new Acceleration(0.0,
5393 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5394 calibrator.getInitialBiasXAsAcceleration(bx2);
5395 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5396 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5397 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5398 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5399 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5400 final Acceleration by2 = new Acceleration(0.0,
5401 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5402 calibrator.getInitialBiasYAsAcceleration(by2);
5403 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5404 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5405 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5406 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5407 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5408 final Acceleration bz2 = new Acceleration(0.0,
5409 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5410 calibrator.getInitialBiasZAsAcceleration(bz2);
5411 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5412 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5413 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5414 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5415 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5416 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5417 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5418 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5419 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5420 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5421 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5422 final double[] bias1 = calibrator.getInitialBias();
5423 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5424 final double[] bias2 = new double[3];
5425 calibrator.getInitialBias(bias2);
5426 assertArrayEquals(bias1, bias2, 0.0);
5427 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5428 assertEquals(b1, ba);
5429 final Matrix b2 = new Matrix(3, 1);
5430 calibrator.getInitialBiasAsMatrix(b2);
5431 assertEquals(b1, b2);
5432 final Matrix ma1 = new Matrix(3, 3);
5433 ma1.setSubmatrix(0, 0,
5434 2, 2,
5435 new double[]{sx, myx, mzx,
5436 mxy, sy, mzy,
5437 mxz, myz, sz});
5438 assertEquals(calibrator.getInitialMa(), ma1);
5439 final Matrix ma2 = new Matrix(3, 3);
5440 calibrator.getInitialMa(ma2);
5441 assertEquals(ma1, ma2);
5442 assertNull(calibrator.getEcefPosition());
5443 assertNull(calibrator.getNedPosition());
5444 assertFalse(calibrator.getNedPosition(null));
5445 assertSame(calibrator.getMeasurements(), measurements);
5446 assertFalse(calibrator.isCommonAxisUsed());
5447 assertNull(calibrator.getListener());
5448 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5449 assertFalse(calibrator.isReady());
5450 assertFalse(calibrator.isRunning());
5451 assertNull(calibrator.getEstimatedBiases());
5452 assertFalse(calibrator.getEstimatedBiases(null));
5453 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5454 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5455 assertNull(calibrator.getEstimatedBiasFx());
5456 assertNull(calibrator.getEstimatedBiasFy());
5457 assertNull(calibrator.getEstimatedBiasFz());
5458 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5459 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5460 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5461 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5462 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5463 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5464 assertNull(calibrator.getEstimatedMa());
5465 assertNull(calibrator.getEstimatedSx());
5466 assertNull(calibrator.getEstimatedSy());
5467 assertNull(calibrator.getEstimatedSz());
5468 assertNull(calibrator.getEstimatedMxy());
5469 assertNull(calibrator.getEstimatedMxz());
5470 assertNull(calibrator.getEstimatedMyx());
5471 assertNull(calibrator.getEstimatedMyz());
5472 assertNull(calibrator.getEstimatedMzx());
5473 assertNull(calibrator.getEstimatedMzy());
5474 assertNull(calibrator.getEstimatedCovariance());
5475 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5476 assertNull(calibrator.getGroundTruthGravityNorm());
5477 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5478 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5479 }
5480
5481 @Test
5482 public void testConstructor50() throws WrongSizeException {
5483
5484 final Collection<StandardDeviationBodyKinematics> measurements =
5485 Collections.emptyList();
5486
5487 final Matrix ba = generateBa();
5488 final double biasX = ba.getElementAtIndex(0);
5489 final double biasY = ba.getElementAtIndex(1);
5490 final double biasZ = ba.getElementAtIndex(2);
5491
5492 final Matrix ma = generateMaCommonAxis();
5493 final double sx = ma.getElementAt(0, 0);
5494 final double sy = ma.getElementAt(1, 1);
5495 final double sz = ma.getElementAt(2, 2);
5496 final double mxy = ma.getElementAt(0, 1);
5497 final double mxz = ma.getElementAt(0, 2);
5498 final double myx = ma.getElementAt(1, 0);
5499 final double myz = ma.getElementAt(1, 2);
5500 final double mzx = ma.getElementAt(2, 0);
5501 final double mzy = ma.getElementAt(2, 1);
5502
5503 final Acceleration bx = new Acceleration(biasX,
5504 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5505 final Acceleration by = new Acceleration(biasY,
5506 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5507 final Acceleration bz = new Acceleration(biasZ,
5508 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5509
5510 final KnownPositionAccelerometerCalibrator calibrator =
5511 new KnownPositionAccelerometerCalibrator(measurements, bx, by, bz,
5512 sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy, this);
5513
5514
5515 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5516 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5517 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5518 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5519 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5520 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5521 final Acceleration bx2 = new Acceleration(0.0,
5522 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5523 calibrator.getInitialBiasXAsAcceleration(bx2);
5524 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5525 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5526 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5527 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5528 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5529 final Acceleration by2 = new Acceleration(0.0,
5530 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5531 calibrator.getInitialBiasYAsAcceleration(by2);
5532 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5533 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5534 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5535 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5536 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5537 final Acceleration bz2 = new Acceleration(0.0,
5538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5539 calibrator.getInitialBiasZAsAcceleration(bz2);
5540 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5541 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5542 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5543 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5544 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5545 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5546 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5547 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5548 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5549 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5550 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5551 final double[] bias1 = calibrator.getInitialBias();
5552 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5553 final double[] bias2 = new double[3];
5554 calibrator.getInitialBias(bias2);
5555 assertArrayEquals(bias1, bias2, 0.0);
5556 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5557 assertEquals(b1, ba);
5558 final Matrix b2 = new Matrix(3, 1);
5559 calibrator.getInitialBiasAsMatrix(b2);
5560 assertEquals(b1, b2);
5561 final Matrix ma1 = new Matrix(3, 3);
5562 ma1.setSubmatrix(0, 0,
5563 2, 2,
5564 new double[]{sx, myx, mzx,
5565 mxy, sy, mzy,
5566 mxz, myz, sz});
5567 assertEquals(calibrator.getInitialMa(), ma1);
5568 final Matrix ma2 = new Matrix(3, 3);
5569 calibrator.getInitialMa(ma2);
5570 assertEquals(ma1, ma2);
5571 assertNull(calibrator.getEcefPosition());
5572 assertNull(calibrator.getNedPosition());
5573 assertFalse(calibrator.getNedPosition(null));
5574 assertSame(calibrator.getMeasurements(), measurements);
5575 assertFalse(calibrator.isCommonAxisUsed());
5576 assertSame(calibrator.getListener(), this);
5577 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
5578 assertFalse(calibrator.isReady());
5579 assertFalse(calibrator.isRunning());
5580 assertNull(calibrator.getEstimatedBiases());
5581 assertFalse(calibrator.getEstimatedBiases(null));
5582 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5583 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5584 assertNull(calibrator.getEstimatedBiasFx());
5585 assertNull(calibrator.getEstimatedBiasFy());
5586 assertNull(calibrator.getEstimatedBiasFz());
5587 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5588 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5589 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5590 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5591 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5592 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5593 assertNull(calibrator.getEstimatedMa());
5594 assertNull(calibrator.getEstimatedSx());
5595 assertNull(calibrator.getEstimatedSy());
5596 assertNull(calibrator.getEstimatedSz());
5597 assertNull(calibrator.getEstimatedMxy());
5598 assertNull(calibrator.getEstimatedMxz());
5599 assertNull(calibrator.getEstimatedMyx());
5600 assertNull(calibrator.getEstimatedMyz());
5601 assertNull(calibrator.getEstimatedMzx());
5602 assertNull(calibrator.getEstimatedMzy());
5603 assertNull(calibrator.getEstimatedCovariance());
5604 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5605 assertNull(calibrator.getGroundTruthGravityNorm());
5606 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5607 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5608 }
5609
5610 @Test
5611 public void testConstructor51() throws WrongSizeException {
5612 final Matrix ba = generateBa();
5613 final double biasX = ba.getElementAtIndex(0);
5614 final double biasY = ba.getElementAtIndex(1);
5615 final double biasZ = ba.getElementAtIndex(2);
5616
5617 final Matrix ma = generateMaCommonAxis();
5618 final double sx = ma.getElementAt(0, 0);
5619 final double sy = ma.getElementAt(1, 1);
5620 final double sz = ma.getElementAt(2, 2);
5621 final double mxy = ma.getElementAt(0, 1);
5622 final double mxz = ma.getElementAt(0, 2);
5623 final double myx = ma.getElementAt(1, 0);
5624 final double myz = ma.getElementAt(1, 2);
5625 final double mzx = ma.getElementAt(2, 0);
5626 final double mzy = ma.getElementAt(2, 1);
5627
5628 final Acceleration bx = new Acceleration(biasX,
5629 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5630 final Acceleration by = new Acceleration(biasY,
5631 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5632 final Acceleration bz = new Acceleration(biasZ,
5633 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5634
5635 final KnownPositionAccelerometerCalibrator calibrator =
5636 new KnownPositionAccelerometerCalibrator(true,
5637 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy);
5638
5639
5640 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5641 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5642 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5643 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5644 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5645 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5646 final Acceleration bx2 = new Acceleration(0.0,
5647 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5648 calibrator.getInitialBiasXAsAcceleration(bx2);
5649 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5650 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5651 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5652 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5653 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5654 final Acceleration by2 = new Acceleration(0.0,
5655 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5656 calibrator.getInitialBiasYAsAcceleration(by2);
5657 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5658 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5659 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5660 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5661 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5662 final Acceleration bz2 = new Acceleration(0.0,
5663 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5664 calibrator.getInitialBiasZAsAcceleration(bz2);
5665 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5666 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5667 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5668 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5669 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5670 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5671 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5672 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5673 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5674 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5675 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5676 final double[] bias1 = calibrator.getInitialBias();
5677 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5678 final double[] bias2 = new double[3];
5679 calibrator.getInitialBias(bias2);
5680 assertArrayEquals(bias1, bias2, 0.0);
5681 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5682 assertEquals(b1, ba);
5683 final Matrix b2 = new Matrix(3, 1);
5684 calibrator.getInitialBiasAsMatrix(b2);
5685 assertEquals(b1, b2);
5686 final Matrix ma1 = new Matrix(3, 3);
5687 ma1.setSubmatrix(0, 0,
5688 2, 2,
5689 new double[]{sx, myx, mzx,
5690 mxy, sy, mzy,
5691 mxz, myz, sz});
5692 assertEquals(calibrator.getInitialMa(), ma1);
5693 final Matrix ma2 = new Matrix(3, 3);
5694 calibrator.getInitialMa(ma2);
5695 assertEquals(ma1, ma2);
5696 assertNull(calibrator.getEcefPosition());
5697 assertNull(calibrator.getNedPosition());
5698 assertFalse(calibrator.getNedPosition(null));
5699 assertNull(calibrator.getMeasurements());
5700 assertTrue(calibrator.isCommonAxisUsed());
5701 assertNull(calibrator.getListener());
5702 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5703 assertFalse(calibrator.isReady());
5704 assertFalse(calibrator.isRunning());
5705 assertNull(calibrator.getEstimatedBiases());
5706 assertFalse(calibrator.getEstimatedBiases(null));
5707 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5708 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5709 assertNull(calibrator.getEstimatedBiasFx());
5710 assertNull(calibrator.getEstimatedBiasFy());
5711 assertNull(calibrator.getEstimatedBiasFz());
5712 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5713 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5714 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5715 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5716 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5717 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5718 assertNull(calibrator.getEstimatedMa());
5719 assertNull(calibrator.getEstimatedSx());
5720 assertNull(calibrator.getEstimatedSy());
5721 assertNull(calibrator.getEstimatedSz());
5722 assertNull(calibrator.getEstimatedMxy());
5723 assertNull(calibrator.getEstimatedMxz());
5724 assertNull(calibrator.getEstimatedMyx());
5725 assertNull(calibrator.getEstimatedMyz());
5726 assertNull(calibrator.getEstimatedMzx());
5727 assertNull(calibrator.getEstimatedMzy());
5728 assertNull(calibrator.getEstimatedCovariance());
5729 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5730 assertNull(calibrator.getGroundTruthGravityNorm());
5731 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5732 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5733 }
5734
5735 @Test
5736 public void testConstructor52() throws WrongSizeException {
5737 final Matrix ba = generateBa();
5738 final double biasX = ba.getElementAtIndex(0);
5739 final double biasY = ba.getElementAtIndex(1);
5740 final double biasZ = ba.getElementAtIndex(2);
5741
5742 final Matrix ma = generateMaCommonAxis();
5743 final double sx = ma.getElementAt(0, 0);
5744 final double sy = ma.getElementAt(1, 1);
5745 final double sz = ma.getElementAt(2, 2);
5746 final double mxy = ma.getElementAt(0, 1);
5747 final double mxz = ma.getElementAt(0, 2);
5748 final double myx = ma.getElementAt(1, 0);
5749 final double myz = ma.getElementAt(1, 2);
5750 final double mzx = ma.getElementAt(2, 0);
5751 final double mzy = ma.getElementAt(2, 1);
5752
5753 final Acceleration bx = new Acceleration(biasX,
5754 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5755 final Acceleration by = new Acceleration(biasY,
5756 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5757 final Acceleration bz = new Acceleration(biasZ,
5758 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5759
5760 final KnownPositionAccelerometerCalibrator calibrator =
5761 new KnownPositionAccelerometerCalibrator(true,
5762 bx, by, bz, sx, sy, sz, mxy, mxz, myx, myz, mzx, mzy,
5763 this);
5764
5765
5766 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5767 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5768 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5769 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5770 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5771 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5772 final Acceleration bx2 = new Acceleration(0.0,
5773 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5774 calibrator.getInitialBiasXAsAcceleration(bx2);
5775 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5776 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5777 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5778 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5779 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5780 final Acceleration by2 = new Acceleration(0.0,
5781 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5782 calibrator.getInitialBiasYAsAcceleration(by2);
5783 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5784 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5785 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5786 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5787 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5788 final Acceleration bz2 = new Acceleration(0.0,
5789 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5790 calibrator.getInitialBiasZAsAcceleration(bz2);
5791 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5792 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5793 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5794 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5795 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5796 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5797 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5798 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5799 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5800 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5801 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5802 final double[] bias1 = calibrator.getInitialBias();
5803 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5804 final double[] bias2 = new double[3];
5805 calibrator.getInitialBias(bias2);
5806 assertArrayEquals(bias1, bias2, 0.0);
5807 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5808 assertEquals(b1, ba);
5809 final Matrix b2 = new Matrix(3, 1);
5810 calibrator.getInitialBiasAsMatrix(b2);
5811 assertEquals(b1, b2);
5812 final Matrix ma1 = new Matrix(3, 3);
5813 ma1.setSubmatrix(0, 0,
5814 2, 2,
5815 new double[]{sx, myx, mzx,
5816 mxy, sy, mzy,
5817 mxz, myz, sz});
5818 assertEquals(calibrator.getInitialMa(), ma1);
5819 final Matrix ma2 = new Matrix(3, 3);
5820 calibrator.getInitialMa(ma2);
5821 assertEquals(ma1, ma2);
5822 assertNull(calibrator.getEcefPosition());
5823 assertNull(calibrator.getNedPosition());
5824 assertFalse(calibrator.getNedPosition(null));
5825 assertNull(calibrator.getMeasurements());
5826 assertTrue(calibrator.isCommonAxisUsed());
5827 assertSame(calibrator.getListener(), this);
5828 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5829 assertFalse(calibrator.isReady());
5830 assertFalse(calibrator.isRunning());
5831 assertNull(calibrator.getEstimatedBiases());
5832 assertFalse(calibrator.getEstimatedBiases(null));
5833 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5834 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5835 assertNull(calibrator.getEstimatedBiasFx());
5836 assertNull(calibrator.getEstimatedBiasFy());
5837 assertNull(calibrator.getEstimatedBiasFz());
5838 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5839 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5840 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5841 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5842 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5843 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5844 assertNull(calibrator.getEstimatedMa());
5845 assertNull(calibrator.getEstimatedSx());
5846 assertNull(calibrator.getEstimatedSy());
5847 assertNull(calibrator.getEstimatedSz());
5848 assertNull(calibrator.getEstimatedMxy());
5849 assertNull(calibrator.getEstimatedMxz());
5850 assertNull(calibrator.getEstimatedMyx());
5851 assertNull(calibrator.getEstimatedMyz());
5852 assertNull(calibrator.getEstimatedMzx());
5853 assertNull(calibrator.getEstimatedMzy());
5854 assertNull(calibrator.getEstimatedCovariance());
5855 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5856 assertNull(calibrator.getGroundTruthGravityNorm());
5857 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5858 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5859 }
5860
5861 @Test
5862 public void testConstructor53() throws WrongSizeException {
5863 final Collection<StandardDeviationBodyKinematics> measurements =
5864 Collections.emptyList();
5865
5866 final Matrix ba = generateBa();
5867 final double biasX = ba.getElementAtIndex(0);
5868 final double biasY = ba.getElementAtIndex(1);
5869 final double biasZ = ba.getElementAtIndex(2);
5870
5871 final Matrix ma = generateMaCommonAxis();
5872 final double sx = ma.getElementAt(0, 0);
5873 final double sy = ma.getElementAt(1, 1);
5874 final double sz = ma.getElementAt(2, 2);
5875 final double mxy = ma.getElementAt(0, 1);
5876 final double mxz = ma.getElementAt(0, 2);
5877 final double myx = ma.getElementAt(1, 0);
5878 final double myz = ma.getElementAt(1, 2);
5879 final double mzx = ma.getElementAt(2, 0);
5880 final double mzy = ma.getElementAt(2, 1);
5881
5882 final Acceleration bx = new Acceleration(biasX,
5883 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5884 final Acceleration by = new Acceleration(biasY,
5885 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5886 final Acceleration bz = new Acceleration(biasZ,
5887 AccelerationUnit.METERS_PER_SQUARED_SECOND);
5888
5889 final KnownPositionAccelerometerCalibrator calibrator =
5890 new KnownPositionAccelerometerCalibrator(measurements,
5891 true, bx, by, bz, sx, sy, sz,
5892 mxy, mxz, myx, myz, mzx, mzy);
5893
5894
5895 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
5896 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
5897 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
5898 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
5899 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
5900 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5901 final Acceleration bx2 = new Acceleration(0.0,
5902 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5903 calibrator.getInitialBiasXAsAcceleration(bx2);
5904 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
5905 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5906 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
5907 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
5908 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5909 final Acceleration by2 = new Acceleration(0.0,
5910 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5911 calibrator.getInitialBiasYAsAcceleration(by2);
5912 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
5913 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5914 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
5915 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
5916 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5917 final Acceleration bz2 = new Acceleration(0.0,
5918 AccelerationUnit.FEET_PER_SQUARED_SECOND);
5919 calibrator.getInitialBiasZAsAcceleration(bz2);
5920 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
5921 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
5922 assertEquals(calibrator.getInitialSx(), sx, 0.0);
5923 assertEquals(calibrator.getInitialSy(), sy, 0.0);
5924 assertEquals(calibrator.getInitialSz(), sz, 0.0);
5925 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
5926 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
5927 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
5928 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
5929 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
5930 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
5931 final double[] bias1 = calibrator.getInitialBias();
5932 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
5933 final double[] bias2 = new double[3];
5934 calibrator.getInitialBias(bias2);
5935 assertArrayEquals(bias1, bias2, 0.0);
5936 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
5937 assertEquals(b1, ba);
5938 final Matrix b2 = new Matrix(3, 1);
5939 calibrator.getInitialBiasAsMatrix(b2);
5940 assertEquals(b1, b2);
5941 final Matrix ma1 = new Matrix(3, 3);
5942 ma1.setSubmatrix(0, 0,
5943 2, 2,
5944 new double[]{sx, myx, mzx,
5945 mxy, sy, mzy,
5946 mxz, myz, sz});
5947 assertEquals(calibrator.getInitialMa(), ma1);
5948 final Matrix ma2 = new Matrix(3, 3);
5949 calibrator.getInitialMa(ma2);
5950 assertEquals(ma1, ma2);
5951 assertNull(calibrator.getEcefPosition());
5952 assertNull(calibrator.getNedPosition());
5953 assertFalse(calibrator.getNedPosition(null));
5954 assertSame(calibrator.getMeasurements(), measurements);
5955 assertTrue(calibrator.isCommonAxisUsed());
5956 assertNull(calibrator.getListener());
5957 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
5958 assertFalse(calibrator.isReady());
5959 assertFalse(calibrator.isRunning());
5960 assertNull(calibrator.getEstimatedBiases());
5961 assertFalse(calibrator.getEstimatedBiases(null));
5962 assertNull(calibrator.getEstimatedBiasesAsMatrix());
5963 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
5964 assertNull(calibrator.getEstimatedBiasFx());
5965 assertNull(calibrator.getEstimatedBiasFy());
5966 assertNull(calibrator.getEstimatedBiasFz());
5967 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
5968 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
5969 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
5970 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
5971 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
5972 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
5973 assertNull(calibrator.getEstimatedMa());
5974 assertNull(calibrator.getEstimatedSx());
5975 assertNull(calibrator.getEstimatedSy());
5976 assertNull(calibrator.getEstimatedSz());
5977 assertNull(calibrator.getEstimatedMxy());
5978 assertNull(calibrator.getEstimatedMxz());
5979 assertNull(calibrator.getEstimatedMyx());
5980 assertNull(calibrator.getEstimatedMyz());
5981 assertNull(calibrator.getEstimatedMzx());
5982 assertNull(calibrator.getEstimatedMzy());
5983 assertNull(calibrator.getEstimatedCovariance());
5984 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
5985 assertNull(calibrator.getGroundTruthGravityNorm());
5986 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
5987 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
5988 }
5989
5990 @Test
5991 public void testConstructor54() throws WrongSizeException {
5992 final Collection<StandardDeviationBodyKinematics> measurements =
5993 Collections.emptyList();
5994
5995 final Matrix ba = generateBa();
5996 final double biasX = ba.getElementAtIndex(0);
5997 final double biasY = ba.getElementAtIndex(1);
5998 final double biasZ = ba.getElementAtIndex(2);
5999
6000 final Matrix ma = generateMaCommonAxis();
6001 final double sx = ma.getElementAt(0, 0);
6002 final double sy = ma.getElementAt(1, 1);
6003 final double sz = ma.getElementAt(2, 2);
6004 final double mxy = ma.getElementAt(0, 1);
6005 final double mxz = ma.getElementAt(0, 2);
6006 final double myx = ma.getElementAt(1, 0);
6007 final double myz = ma.getElementAt(1, 2);
6008 final double mzx = ma.getElementAt(2, 0);
6009 final double mzy = ma.getElementAt(2, 1);
6010
6011 final Acceleration bx = new Acceleration(biasX,
6012 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6013 final Acceleration by = new Acceleration(biasY,
6014 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6015 final Acceleration bz = new Acceleration(biasZ,
6016 AccelerationUnit.METERS_PER_SQUARED_SECOND);
6017
6018 final KnownPositionAccelerometerCalibrator calibrator =
6019 new KnownPositionAccelerometerCalibrator(measurements,
6020 true, bx, by, bz, sx, sy, sz,
6021 mxy, mxz, myx, myz, mzx, mzy, this);
6022
6023
6024 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6025 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6026 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6027 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6028 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6029 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6030 final Acceleration bx2 = new Acceleration(0.0,
6031 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6032 calibrator.getInitialBiasXAsAcceleration(bx2);
6033 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6034 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6035 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6036 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6037 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6038 final Acceleration by2 = new Acceleration(0.0,
6039 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6040 calibrator.getInitialBiasYAsAcceleration(by2);
6041 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6042 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6043 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6044 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6045 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6046 final Acceleration bz2 = new Acceleration(0.0,
6047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6048 calibrator.getInitialBiasZAsAcceleration(bz2);
6049 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6050 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6051 assertEquals(calibrator.getInitialSx(), sx, 0.0);
6052 assertEquals(calibrator.getInitialSy(), sy, 0.0);
6053 assertEquals(calibrator.getInitialSz(), sz, 0.0);
6054 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
6055 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
6056 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
6057 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
6058 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
6059 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
6060 final double[] bias1 = calibrator.getInitialBias();
6061 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6062 final double[] bias2 = new double[3];
6063 calibrator.getInitialBias(bias2);
6064 assertArrayEquals(bias1, bias2, 0.0);
6065 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6066 assertEquals(b1, ba);
6067 final Matrix b2 = new Matrix(3, 1);
6068 calibrator.getInitialBiasAsMatrix(b2);
6069 assertEquals(b1, b2);
6070 final Matrix ma1 = new Matrix(3, 3);
6071 ma1.setSubmatrix(0, 0,
6072 2, 2,
6073 new double[]{sx, myx, mzx,
6074 mxy, sy, mzy,
6075 mxz, myz, sz});
6076 assertEquals(calibrator.getInitialMa(), ma1);
6077 final Matrix ma2 = new Matrix(3, 3);
6078 calibrator.getInitialMa(ma2);
6079 assertEquals(ma1, ma2);
6080 assertNull(calibrator.getEcefPosition());
6081 assertNull(calibrator.getNedPosition());
6082 assertFalse(calibrator.getNedPosition(null));
6083 assertSame(calibrator.getMeasurements(), measurements);
6084 assertTrue(calibrator.isCommonAxisUsed());
6085 assertSame(calibrator.getListener(), this);
6086 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6087 assertFalse(calibrator.isReady());
6088 assertFalse(calibrator.isRunning());
6089 assertNull(calibrator.getEstimatedBiases());
6090 assertFalse(calibrator.getEstimatedBiases(null));
6091 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6092 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6093 assertNull(calibrator.getEstimatedBiasFx());
6094 assertNull(calibrator.getEstimatedBiasFy());
6095 assertNull(calibrator.getEstimatedBiasFz());
6096 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6097 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6098 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6099 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6100 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6101 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6102 assertNull(calibrator.getEstimatedMa());
6103 assertNull(calibrator.getEstimatedSx());
6104 assertNull(calibrator.getEstimatedSy());
6105 assertNull(calibrator.getEstimatedSz());
6106 assertNull(calibrator.getEstimatedMxy());
6107 assertNull(calibrator.getEstimatedMxz());
6108 assertNull(calibrator.getEstimatedMyx());
6109 assertNull(calibrator.getEstimatedMyz());
6110 assertNull(calibrator.getEstimatedMzx());
6111 assertNull(calibrator.getEstimatedMzy());
6112 assertNull(calibrator.getEstimatedCovariance());
6113 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6114 assertNull(calibrator.getGroundTruthGravityNorm());
6115 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6116 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6117 }
6118
6119 @Test
6120 public void testConstructor55() throws WrongSizeException {
6121 final Matrix ba = generateBa();
6122 final double[] bias = ba.getBuffer();
6123 final double biasX = ba.getElementAtIndex(0);
6124 final double biasY = ba.getElementAtIndex(1);
6125 final double biasZ = ba.getElementAtIndex(2);
6126
6127 KnownPositionAccelerometerCalibrator calibrator =
6128 new KnownPositionAccelerometerCalibrator(bias);
6129
6130
6131 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6132 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6133 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6134 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6135 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6136 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6137 final Acceleration bx2 = new Acceleration(0.0,
6138 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6139 calibrator.getInitialBiasXAsAcceleration(bx2);
6140 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6141 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6142 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6143 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6144 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6145 final Acceleration by2 = new Acceleration(0.0,
6146 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6147 calibrator.getInitialBiasYAsAcceleration(by2);
6148 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6149 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6150 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6151 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6152 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6153 final Acceleration bz2 = new Acceleration(0.0,
6154 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6155 calibrator.getInitialBiasZAsAcceleration(bz2);
6156 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6157 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6158 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6159 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6160 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6161 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6162 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6163 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6164 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6165 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6166 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6167 final double[] bias1 = calibrator.getInitialBias();
6168 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6169 final double[] bias2 = new double[3];
6170 calibrator.getInitialBias(bias2);
6171 assertArrayEquals(bias1, bias2, 0.0);
6172 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6173 assertEquals(b1, ba);
6174 final Matrix b2 = new Matrix(3, 1);
6175 calibrator.getInitialBiasAsMatrix(b2);
6176 assertEquals(b1, b2);
6177 final Matrix ma1 = calibrator.getInitialMa();
6178 assertEquals(ma1, new Matrix(3, 3));
6179 final Matrix ma2 = new Matrix(3, 3);
6180 calibrator.getInitialMa(ma2);
6181 assertEquals(ma1, ma2);
6182 assertNull(calibrator.getEcefPosition());
6183 assertNull(calibrator.getNedPosition());
6184 assertFalse(calibrator.getNedPosition(null));
6185 assertNull(calibrator.getMeasurements());
6186 assertFalse(calibrator.isCommonAxisUsed());
6187 assertNull(calibrator.getListener());
6188 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6189 assertFalse(calibrator.isReady());
6190 assertFalse(calibrator.isRunning());
6191 assertNull(calibrator.getEstimatedBiases());
6192 assertFalse(calibrator.getEstimatedBiases(null));
6193 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6194 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6195 assertNull(calibrator.getEstimatedBiasFx());
6196 assertNull(calibrator.getEstimatedBiasFy());
6197 assertNull(calibrator.getEstimatedBiasFz());
6198 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6199 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6200 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6201 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6202 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6203 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6204 assertNull(calibrator.getEstimatedMa());
6205 assertNull(calibrator.getEstimatedSx());
6206 assertNull(calibrator.getEstimatedSy());
6207 assertNull(calibrator.getEstimatedSz());
6208 assertNull(calibrator.getEstimatedMxy());
6209 assertNull(calibrator.getEstimatedMxz());
6210 assertNull(calibrator.getEstimatedMyx());
6211 assertNull(calibrator.getEstimatedMyz());
6212 assertNull(calibrator.getEstimatedMzx());
6213 assertNull(calibrator.getEstimatedMzy());
6214 assertNull(calibrator.getEstimatedCovariance());
6215 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6216 assertNull(calibrator.getGroundTruthGravityNorm());
6217 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6218 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6219
6220
6221 calibrator = null;
6222 try {
6223 calibrator = new KnownPositionAccelerometerCalibrator(new double[1]);
6224 fail("IllegalArgumentException expected but not thrown");
6225 } catch (final IllegalArgumentException ignore) {
6226 }
6227 assertNull(calibrator);
6228 }
6229
6230 @Test
6231 public void testConstructor56() throws WrongSizeException {
6232 final Matrix ba = generateBa();
6233 final double[] bias = ba.getBuffer();
6234 final double biasX = ba.getElementAtIndex(0);
6235 final double biasY = ba.getElementAtIndex(1);
6236 final double biasZ = ba.getElementAtIndex(2);
6237
6238 KnownPositionAccelerometerCalibrator calibrator =
6239 new KnownPositionAccelerometerCalibrator(bias, this);
6240
6241
6242 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6243 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6244 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6245 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6246 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6247 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6248 final Acceleration bx2 = new Acceleration(0.0,
6249 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6250 calibrator.getInitialBiasXAsAcceleration(bx2);
6251 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6252 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6253 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6254 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6255 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6256 final Acceleration by2 = new Acceleration(0.0,
6257 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6258 calibrator.getInitialBiasYAsAcceleration(by2);
6259 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6260 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6261 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6262 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6263 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6264 final Acceleration bz2 = new Acceleration(0.0,
6265 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6266 calibrator.getInitialBiasZAsAcceleration(bz2);
6267 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6268 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6269 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6270 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6271 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6272 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6273 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6274 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6275 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6276 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6277 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6278 final double[] bias1 = calibrator.getInitialBias();
6279 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6280 final double[] bias2 = new double[3];
6281 calibrator.getInitialBias(bias2);
6282 assertArrayEquals(bias1, bias2, 0.0);
6283 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6284 assertEquals(b1, ba);
6285 final Matrix b2 = new Matrix(3, 1);
6286 calibrator.getInitialBiasAsMatrix(b2);
6287 assertEquals(b1, b2);
6288 final Matrix ma1 = calibrator.getInitialMa();
6289 assertEquals(ma1, new Matrix(3, 3));
6290 final Matrix ma2 = new Matrix(3, 3);
6291 calibrator.getInitialMa(ma2);
6292 assertEquals(ma1, ma2);
6293 assertNull(calibrator.getEcefPosition());
6294 assertNull(calibrator.getNedPosition());
6295 assertFalse(calibrator.getNedPosition(null));
6296 assertNull(calibrator.getMeasurements());
6297 assertFalse(calibrator.isCommonAxisUsed());
6298 assertSame(calibrator.getListener(), this);
6299 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6300 assertFalse(calibrator.isReady());
6301 assertFalse(calibrator.isRunning());
6302 assertNull(calibrator.getEstimatedBiases());
6303 assertFalse(calibrator.getEstimatedBiases(null));
6304 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6305 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6306 assertNull(calibrator.getEstimatedBiasFx());
6307 assertNull(calibrator.getEstimatedBiasFy());
6308 assertNull(calibrator.getEstimatedBiasFz());
6309 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6310 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6311 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6312 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6313 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6314 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6315 assertNull(calibrator.getEstimatedMa());
6316 assertNull(calibrator.getEstimatedSx());
6317 assertNull(calibrator.getEstimatedSy());
6318 assertNull(calibrator.getEstimatedSz());
6319 assertNull(calibrator.getEstimatedMxy());
6320 assertNull(calibrator.getEstimatedMxz());
6321 assertNull(calibrator.getEstimatedMyx());
6322 assertNull(calibrator.getEstimatedMyz());
6323 assertNull(calibrator.getEstimatedMzx());
6324 assertNull(calibrator.getEstimatedMzy());
6325 assertNull(calibrator.getEstimatedCovariance());
6326 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6327 assertNull(calibrator.getGroundTruthGravityNorm());
6328 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6329 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6330
6331
6332 calibrator = null;
6333 try {
6334 calibrator = new KnownPositionAccelerometerCalibrator(new double[1],
6335 this);
6336 fail("IllegalArgumentException expected but not thrown");
6337 } catch (final IllegalArgumentException ignore) {
6338 }
6339 assertNull(calibrator);
6340 }
6341
6342 @Test
6343 public void testConstructor57() throws WrongSizeException {
6344 final Collection<StandardDeviationBodyKinematics> measurements =
6345 Collections.emptyList();
6346
6347 final Matrix ba = generateBa();
6348 final double[] bias = ba.getBuffer();
6349 final double biasX = ba.getElementAtIndex(0);
6350 final double biasY = ba.getElementAtIndex(1);
6351 final double biasZ = ba.getElementAtIndex(2);
6352
6353 KnownPositionAccelerometerCalibrator calibrator =
6354 new KnownPositionAccelerometerCalibrator(measurements, bias);
6355
6356
6357 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6358 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6359 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6360 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6361 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6362 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6363 final Acceleration bx2 = new Acceleration(0.0,
6364 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6365 calibrator.getInitialBiasXAsAcceleration(bx2);
6366 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6367 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6368 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6369 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6370 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6371 final Acceleration by2 = new Acceleration(0.0,
6372 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6373 calibrator.getInitialBiasYAsAcceleration(by2);
6374 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6375 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6376 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6377 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6378 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6379 final Acceleration bz2 = new Acceleration(0.0,
6380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6381 calibrator.getInitialBiasZAsAcceleration(bz2);
6382 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6383 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6384 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6385 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6386 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6387 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6388 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6389 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6390 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6391 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6392 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6393 final double[] bias1 = calibrator.getInitialBias();
6394 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6395 final double[] bias2 = new double[3];
6396 calibrator.getInitialBias(bias2);
6397 assertArrayEquals(bias1, bias2, 0.0);
6398 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6399 assertEquals(b1, ba);
6400 final Matrix b2 = new Matrix(3, 1);
6401 calibrator.getInitialBiasAsMatrix(b2);
6402 assertEquals(b1, b2);
6403 final Matrix ma1 = calibrator.getInitialMa();
6404 assertEquals(ma1, new Matrix(3, 3));
6405 final Matrix ma2 = new Matrix(3, 3);
6406 calibrator.getInitialMa(ma2);
6407 assertEquals(ma1, ma2);
6408 assertNull(calibrator.getEcefPosition());
6409 assertNull(calibrator.getNedPosition());
6410 assertFalse(calibrator.getNedPosition(null));
6411 assertSame(calibrator.getMeasurements(), measurements);
6412 assertFalse(calibrator.isCommonAxisUsed());
6413 assertNull(calibrator.getListener());
6414 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6415 assertFalse(calibrator.isReady());
6416 assertFalse(calibrator.isRunning());
6417 assertNull(calibrator.getEstimatedBiases());
6418 assertFalse(calibrator.getEstimatedBiases(null));
6419 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6420 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6421 assertNull(calibrator.getEstimatedBiasFx());
6422 assertNull(calibrator.getEstimatedBiasFy());
6423 assertNull(calibrator.getEstimatedBiasFz());
6424 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6425 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6426 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6427 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6428 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6429 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6430 assertNull(calibrator.getEstimatedMa());
6431 assertNull(calibrator.getEstimatedSx());
6432 assertNull(calibrator.getEstimatedSy());
6433 assertNull(calibrator.getEstimatedSz());
6434 assertNull(calibrator.getEstimatedMxy());
6435 assertNull(calibrator.getEstimatedMxz());
6436 assertNull(calibrator.getEstimatedMyx());
6437 assertNull(calibrator.getEstimatedMyz());
6438 assertNull(calibrator.getEstimatedMzx());
6439 assertNull(calibrator.getEstimatedMzy());
6440 assertNull(calibrator.getEstimatedCovariance());
6441 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6442 assertNull(calibrator.getGroundTruthGravityNorm());
6443 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6444 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6445
6446
6447 calibrator = null;
6448 try {
6449 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6450 new double[1]);
6451 fail("IllegalArgumentException expected but not thrown");
6452 } catch (final IllegalArgumentException ignore) {
6453 }
6454 assertNull(calibrator);
6455 }
6456
6457 @Test
6458 public void testConstructor58() throws WrongSizeException {
6459 final Collection<StandardDeviationBodyKinematics> measurements =
6460 Collections.emptyList();
6461
6462 final Matrix ba = generateBa();
6463 final double[] bias = ba.getBuffer();
6464 final double biasX = ba.getElementAtIndex(0);
6465 final double biasY = ba.getElementAtIndex(1);
6466 final double biasZ = ba.getElementAtIndex(2);
6467
6468 KnownPositionAccelerometerCalibrator calibrator =
6469 new KnownPositionAccelerometerCalibrator(measurements, bias,
6470 this);
6471
6472
6473 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6474 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6475 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6476 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6477 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6478 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6479 final Acceleration bx2 = new Acceleration(0.0,
6480 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6481 calibrator.getInitialBiasXAsAcceleration(bx2);
6482 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6483 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6484 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6485 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6486 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6487 final Acceleration by2 = new Acceleration(0.0,
6488 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6489 calibrator.getInitialBiasYAsAcceleration(by2);
6490 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6491 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6492 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6493 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6494 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6495 final Acceleration bz2 = new Acceleration(0.0,
6496 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6497 calibrator.getInitialBiasZAsAcceleration(bz2);
6498 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6499 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6500 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6501 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6502 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6503 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6504 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6505 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6506 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6507 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6508 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6509 final double[] bias1 = calibrator.getInitialBias();
6510 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6511 final double[] bias2 = new double[3];
6512 calibrator.getInitialBias(bias2);
6513 assertArrayEquals(bias1, bias2, 0.0);
6514 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6515 assertEquals(b1, ba);
6516 final Matrix b2 = new Matrix(3, 1);
6517 calibrator.getInitialBiasAsMatrix(b2);
6518 assertEquals(b1, b2);
6519 final Matrix ma1 = calibrator.getInitialMa();
6520 assertEquals(ma1, new Matrix(3, 3));
6521 final Matrix ma2 = new Matrix(3, 3);
6522 calibrator.getInitialMa(ma2);
6523 assertEquals(ma1, ma2);
6524 assertNull(calibrator.getEcefPosition());
6525 assertNull(calibrator.getNedPosition());
6526 assertFalse(calibrator.getNedPosition(null));
6527 assertSame(calibrator.getMeasurements(), measurements);
6528 assertFalse(calibrator.isCommonAxisUsed());
6529 assertSame(calibrator.getListener(), this);
6530 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
6531 assertFalse(calibrator.isReady());
6532 assertFalse(calibrator.isRunning());
6533 assertNull(calibrator.getEstimatedBiases());
6534 assertFalse(calibrator.getEstimatedBiases(null));
6535 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6536 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6537 assertNull(calibrator.getEstimatedBiasFx());
6538 assertNull(calibrator.getEstimatedBiasFy());
6539 assertNull(calibrator.getEstimatedBiasFz());
6540 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6541 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6542 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6543 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6544 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6545 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6546 assertNull(calibrator.getEstimatedMa());
6547 assertNull(calibrator.getEstimatedSx());
6548 assertNull(calibrator.getEstimatedSy());
6549 assertNull(calibrator.getEstimatedSz());
6550 assertNull(calibrator.getEstimatedMxy());
6551 assertNull(calibrator.getEstimatedMxz());
6552 assertNull(calibrator.getEstimatedMyx());
6553 assertNull(calibrator.getEstimatedMyz());
6554 assertNull(calibrator.getEstimatedMzx());
6555 assertNull(calibrator.getEstimatedMzy());
6556 assertNull(calibrator.getEstimatedCovariance());
6557 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6558 assertNull(calibrator.getGroundTruthGravityNorm());
6559 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6560 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6561
6562
6563 calibrator = null;
6564 try {
6565 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6566 new double[1], this);
6567 fail("IllegalArgumentException expected but not thrown");
6568 } catch (final IllegalArgumentException ignore) {
6569 }
6570 assertNull(calibrator);
6571 }
6572
6573 @Test
6574 public void testConstructor59() throws WrongSizeException {
6575 final Matrix ba = generateBa();
6576 final double[] bias = ba.getBuffer();
6577 final double biasX = ba.getElementAtIndex(0);
6578 final double biasY = ba.getElementAtIndex(1);
6579 final double biasZ = ba.getElementAtIndex(2);
6580
6581 KnownPositionAccelerometerCalibrator calibrator =
6582 new KnownPositionAccelerometerCalibrator(true, bias);
6583
6584
6585 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6586 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6587 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6588 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6589 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6590 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6591 final Acceleration bx2 = new Acceleration(0.0,
6592 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6593 calibrator.getInitialBiasXAsAcceleration(bx2);
6594 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6595 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6596 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6597 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6598 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6599 final Acceleration by2 = new Acceleration(0.0,
6600 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6601 calibrator.getInitialBiasYAsAcceleration(by2);
6602 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6603 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6604 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6605 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6606 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6607 final Acceleration bz2 = new Acceleration(0.0,
6608 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6609 calibrator.getInitialBiasZAsAcceleration(bz2);
6610 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6611 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6612 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6613 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6614 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6615 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6616 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6617 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6618 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6619 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6620 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6621 final double[] bias1 = calibrator.getInitialBias();
6622 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6623 final double[] bias2 = new double[3];
6624 calibrator.getInitialBias(bias2);
6625 assertArrayEquals(bias1, bias2, 0.0);
6626 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6627 assertEquals(b1, ba);
6628 final Matrix b2 = new Matrix(3, 1);
6629 calibrator.getInitialBiasAsMatrix(b2);
6630 assertEquals(b1, b2);
6631 final Matrix ma1 = calibrator.getInitialMa();
6632 assertEquals(ma1, new Matrix(3, 3));
6633 final Matrix ma2 = new Matrix(3, 3);
6634 calibrator.getInitialMa(ma2);
6635 assertEquals(ma1, ma2);
6636 assertNull(calibrator.getEcefPosition());
6637 assertNull(calibrator.getNedPosition());
6638 assertFalse(calibrator.getNedPosition(null));
6639 assertNull(calibrator.getMeasurements());
6640 assertTrue(calibrator.isCommonAxisUsed());
6641 assertNull(calibrator.getListener());
6642 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6643 assertFalse(calibrator.isReady());
6644 assertFalse(calibrator.isRunning());
6645 assertNull(calibrator.getEstimatedBiases());
6646 assertFalse(calibrator.getEstimatedBiases(null));
6647 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6648 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6649 assertNull(calibrator.getEstimatedBiasFx());
6650 assertNull(calibrator.getEstimatedBiasFy());
6651 assertNull(calibrator.getEstimatedBiasFz());
6652 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6653 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6654 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6655 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6656 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6657 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6658 assertNull(calibrator.getEstimatedMa());
6659 assertNull(calibrator.getEstimatedSx());
6660 assertNull(calibrator.getEstimatedSy());
6661 assertNull(calibrator.getEstimatedSz());
6662 assertNull(calibrator.getEstimatedMxy());
6663 assertNull(calibrator.getEstimatedMxz());
6664 assertNull(calibrator.getEstimatedMyx());
6665 assertNull(calibrator.getEstimatedMyz());
6666 assertNull(calibrator.getEstimatedMzx());
6667 assertNull(calibrator.getEstimatedMzy());
6668 assertNull(calibrator.getEstimatedCovariance());
6669 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6670 assertNull(calibrator.getGroundTruthGravityNorm());
6671 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6672 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6673
6674
6675 calibrator = null;
6676 try {
6677 calibrator = new KnownPositionAccelerometerCalibrator(true,
6678 new double[1]);
6679 fail("IllegalArgumentException expected but not thrown");
6680 } catch (final IllegalArgumentException ignore) {
6681 }
6682 assertNull(calibrator);
6683 }
6684
6685 @Test
6686 public void testConstructor60() throws WrongSizeException {
6687 final Matrix ba = generateBa();
6688 final double[] bias = ba.getBuffer();
6689 final double biasX = ba.getElementAtIndex(0);
6690 final double biasY = ba.getElementAtIndex(1);
6691 final double biasZ = ba.getElementAtIndex(2);
6692
6693 KnownPositionAccelerometerCalibrator calibrator =
6694 new KnownPositionAccelerometerCalibrator(true, bias, this);
6695
6696
6697 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6698 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6699 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6700 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6701 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6702 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6703 final Acceleration bx2 = new Acceleration(0.0,
6704 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6705 calibrator.getInitialBiasXAsAcceleration(bx2);
6706 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6707 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6708 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6709 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6710 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6711 final Acceleration by2 = new Acceleration(0.0,
6712 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6713 calibrator.getInitialBiasYAsAcceleration(by2);
6714 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6715 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6716 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6717 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6718 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6719 final Acceleration bz2 = new Acceleration(0.0,
6720 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6721 calibrator.getInitialBiasZAsAcceleration(bz2);
6722 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6723 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6724 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6725 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6726 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6727 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6728 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6729 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6730 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6731 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6732 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6733 final double[] bias1 = calibrator.getInitialBias();
6734 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6735 final double[] bias2 = new double[3];
6736 calibrator.getInitialBias(bias2);
6737 assertArrayEquals(bias1, bias2, 0.0);
6738 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6739 assertEquals(b1, ba);
6740 final Matrix b2 = new Matrix(3, 1);
6741 calibrator.getInitialBiasAsMatrix(b2);
6742 assertEquals(b1, b2);
6743 final Matrix ma1 = calibrator.getInitialMa();
6744 assertEquals(ma1, new Matrix(3, 3));
6745 final Matrix ma2 = new Matrix(3, 3);
6746 calibrator.getInitialMa(ma2);
6747 assertEquals(ma1, ma2);
6748 assertNull(calibrator.getEcefPosition());
6749 assertNull(calibrator.getNedPosition());
6750 assertFalse(calibrator.getNedPosition(null));
6751 assertNull(calibrator.getMeasurements());
6752 assertTrue(calibrator.isCommonAxisUsed());
6753 assertSame(calibrator.getListener(), this);
6754 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6755 assertFalse(calibrator.isReady());
6756 assertFalse(calibrator.isRunning());
6757 assertNull(calibrator.getEstimatedBiases());
6758 assertFalse(calibrator.getEstimatedBiases(null));
6759 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6760 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6761 assertNull(calibrator.getEstimatedBiasFx());
6762 assertNull(calibrator.getEstimatedBiasFy());
6763 assertNull(calibrator.getEstimatedBiasFz());
6764 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6765 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6766 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6767 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6768 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6769 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6770 assertNull(calibrator.getEstimatedMa());
6771 assertNull(calibrator.getEstimatedSx());
6772 assertNull(calibrator.getEstimatedSy());
6773 assertNull(calibrator.getEstimatedSz());
6774 assertNull(calibrator.getEstimatedMxy());
6775 assertNull(calibrator.getEstimatedMxz());
6776 assertNull(calibrator.getEstimatedMyx());
6777 assertNull(calibrator.getEstimatedMyz());
6778 assertNull(calibrator.getEstimatedMzx());
6779 assertNull(calibrator.getEstimatedMzy());
6780 assertNull(calibrator.getEstimatedCovariance());
6781 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6782 assertNull(calibrator.getGroundTruthGravityNorm());
6783 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6784 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6785
6786
6787 calibrator = null;
6788 try {
6789 calibrator = new KnownPositionAccelerometerCalibrator(true,
6790 new double[1], this);
6791 fail("IllegalArgumentException expected but not thrown");
6792 } catch (final IllegalArgumentException ignore) {
6793 }
6794 assertNull(calibrator);
6795 }
6796
6797 @Test
6798 public void testConstructor61() throws WrongSizeException {
6799 final Collection<StandardDeviationBodyKinematics> measurements =
6800 Collections.emptyList();
6801
6802 final Matrix ba = generateBa();
6803 final double[] bias = ba.getBuffer();
6804 final double biasX = ba.getElementAtIndex(0);
6805 final double biasY = ba.getElementAtIndex(1);
6806 final double biasZ = ba.getElementAtIndex(2);
6807
6808 KnownPositionAccelerometerCalibrator calibrator =
6809 new KnownPositionAccelerometerCalibrator(measurements,
6810 true, bias);
6811
6812
6813 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6814 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6815 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6816 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6817 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6818 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6819 final Acceleration bx2 = new Acceleration(0.0,
6820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6821 calibrator.getInitialBiasXAsAcceleration(bx2);
6822 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6823 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6824 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6825 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6826 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6827 final Acceleration by2 = new Acceleration(0.0,
6828 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6829 calibrator.getInitialBiasYAsAcceleration(by2);
6830 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6831 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6832 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6833 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6834 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6835 final Acceleration bz2 = new Acceleration(0.0,
6836 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6837 calibrator.getInitialBiasZAsAcceleration(bz2);
6838 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6839 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6840 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6841 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6842 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6843 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6844 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6845 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6846 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6847 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6848 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6849 final double[] bias1 = calibrator.getInitialBias();
6850 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6851 final double[] bias2 = new double[3];
6852 calibrator.getInitialBias(bias2);
6853 assertArrayEquals(bias1, bias2, 0.0);
6854 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6855 assertEquals(b1, ba);
6856 final Matrix b2 = new Matrix(3, 1);
6857 calibrator.getInitialBiasAsMatrix(b2);
6858 assertEquals(b1, b2);
6859 final Matrix ma1 = calibrator.getInitialMa();
6860 assertEquals(ma1, new Matrix(3, 3));
6861 final Matrix ma2 = new Matrix(3, 3);
6862 calibrator.getInitialMa(ma2);
6863 assertEquals(ma1, ma2);
6864 assertNull(calibrator.getEcefPosition());
6865 assertNull(calibrator.getNedPosition());
6866 assertFalse(calibrator.getNedPosition(null));
6867 assertSame(calibrator.getMeasurements(), measurements);
6868 assertTrue(calibrator.isCommonAxisUsed());
6869 assertNull(calibrator.getListener());
6870 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6871 assertFalse(calibrator.isReady());
6872 assertFalse(calibrator.isRunning());
6873 assertNull(calibrator.getEstimatedBiases());
6874 assertFalse(calibrator.getEstimatedBiases(null));
6875 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6876 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6877 assertNull(calibrator.getEstimatedBiasFx());
6878 assertNull(calibrator.getEstimatedBiasFy());
6879 assertNull(calibrator.getEstimatedBiasFz());
6880 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6881 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6882 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6883 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
6884 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
6885 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
6886 assertNull(calibrator.getEstimatedMa());
6887 assertNull(calibrator.getEstimatedSx());
6888 assertNull(calibrator.getEstimatedSy());
6889 assertNull(calibrator.getEstimatedSz());
6890 assertNull(calibrator.getEstimatedMxy());
6891 assertNull(calibrator.getEstimatedMxz());
6892 assertNull(calibrator.getEstimatedMyx());
6893 assertNull(calibrator.getEstimatedMyz());
6894 assertNull(calibrator.getEstimatedMzx());
6895 assertNull(calibrator.getEstimatedMzy());
6896 assertNull(calibrator.getEstimatedCovariance());
6897 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
6898 assertNull(calibrator.getGroundTruthGravityNorm());
6899 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
6900 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
6901
6902
6903 calibrator = null;
6904 try {
6905 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
6906 true, new double[1]);
6907 fail("IllegalArgumentException expected but not thrown");
6908 } catch (final IllegalArgumentException ignore) {
6909 }
6910 assertNull(calibrator);
6911 }
6912
6913 @Test
6914 public void testConstructor62() throws WrongSizeException {
6915 final Collection<StandardDeviationBodyKinematics> measurements =
6916 Collections.emptyList();
6917
6918 final Matrix ba = generateBa();
6919 final double[] bias = ba.getBuffer();
6920 final double biasX = ba.getElementAtIndex(0);
6921 final double biasY = ba.getElementAtIndex(1);
6922 final double biasZ = ba.getElementAtIndex(2);
6923
6924 KnownPositionAccelerometerCalibrator calibrator =
6925 new KnownPositionAccelerometerCalibrator(measurements,
6926 true, bias, this);
6927
6928
6929 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
6930 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
6931 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
6932 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
6933 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
6934 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6935 final Acceleration bx2 = new Acceleration(0.0,
6936 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6937 calibrator.getInitialBiasXAsAcceleration(bx2);
6938 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
6939 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6940 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
6941 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
6942 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6943 final Acceleration by2 = new Acceleration(0.0,
6944 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6945 calibrator.getInitialBiasYAsAcceleration(by2);
6946 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
6947 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6948 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
6949 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
6950 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6951 final Acceleration bz2 = new Acceleration(0.0,
6952 AccelerationUnit.FEET_PER_SQUARED_SECOND);
6953 calibrator.getInitialBiasZAsAcceleration(bz2);
6954 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
6955 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
6956 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
6957 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
6958 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
6959 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
6960 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
6961 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
6962 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
6963 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
6964 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
6965 final double[] bias1 = calibrator.getInitialBias();
6966 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
6967 final double[] bias2 = new double[3];
6968 calibrator.getInitialBias(bias2);
6969 assertArrayEquals(bias1, bias2, 0.0);
6970 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
6971 assertEquals(b1, ba);
6972 final Matrix b2 = new Matrix(3, 1);
6973 calibrator.getInitialBiasAsMatrix(b2);
6974 assertEquals(b1, b2);
6975 final Matrix ma1 = calibrator.getInitialMa();
6976 assertEquals(ma1, new Matrix(3, 3));
6977 final Matrix ma2 = new Matrix(3, 3);
6978 calibrator.getInitialMa(ma2);
6979 assertEquals(ma1, ma2);
6980 assertNull(calibrator.getEcefPosition());
6981 assertNull(calibrator.getNedPosition());
6982 assertFalse(calibrator.getNedPosition(null));
6983 assertSame(calibrator.getMeasurements(), measurements);
6984 assertTrue(calibrator.isCommonAxisUsed());
6985 assertSame(calibrator.getListener(), this);
6986 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
6987 assertFalse(calibrator.isReady());
6988 assertFalse(calibrator.isRunning());
6989 assertNull(calibrator.getEstimatedBiases());
6990 assertFalse(calibrator.getEstimatedBiases(null));
6991 assertNull(calibrator.getEstimatedBiasesAsMatrix());
6992 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
6993 assertNull(calibrator.getEstimatedBiasFx());
6994 assertNull(calibrator.getEstimatedBiasFy());
6995 assertNull(calibrator.getEstimatedBiasFz());
6996 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
6997 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
6998 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
6999 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7000 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7001 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7002 assertNull(calibrator.getEstimatedMa());
7003 assertNull(calibrator.getEstimatedSx());
7004 assertNull(calibrator.getEstimatedSy());
7005 assertNull(calibrator.getEstimatedSz());
7006 assertNull(calibrator.getEstimatedMxy());
7007 assertNull(calibrator.getEstimatedMxz());
7008 assertNull(calibrator.getEstimatedMyx());
7009 assertNull(calibrator.getEstimatedMyz());
7010 assertNull(calibrator.getEstimatedMzx());
7011 assertNull(calibrator.getEstimatedMzy());
7012 assertNull(calibrator.getEstimatedCovariance());
7013 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7014 assertNull(calibrator.getGroundTruthGravityNorm());
7015 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7016 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7017
7018
7019 calibrator = null;
7020 try {
7021 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7022 true, new double[1], this);
7023 fail("IllegalArgumentException expected but not thrown");
7024 } catch (final IllegalArgumentException ignore) {
7025 }
7026 assertNull(calibrator);
7027 }
7028
7029 @Test
7030 public void testConstructor63() throws WrongSizeException {
7031 final Matrix ba = generateBa();
7032 final double biasX = ba.getElementAtIndex(0);
7033 final double biasY = ba.getElementAtIndex(1);
7034 final double biasZ = ba.getElementAtIndex(2);
7035
7036 KnownPositionAccelerometerCalibrator calibrator =
7037 new KnownPositionAccelerometerCalibrator(ba);
7038
7039
7040 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7041 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7042 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7043 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7044 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7045 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7046 final Acceleration bx2 = new Acceleration(0.0,
7047 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7048 calibrator.getInitialBiasXAsAcceleration(bx2);
7049 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7050 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7051 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7052 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7053 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7054 final Acceleration by2 = new Acceleration(0.0,
7055 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7056 calibrator.getInitialBiasYAsAcceleration(by2);
7057 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7058 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7059 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7060 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7061 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7062 final Acceleration bz2 = new Acceleration(0.0,
7063 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7064 calibrator.getInitialBiasZAsAcceleration(bz2);
7065 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7066 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7067 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7068 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7069 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7070 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7071 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7072 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7073 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7074 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7075 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7076 final double[] bias1 = calibrator.getInitialBias();
7077 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7078 final double[] bias2 = new double[3];
7079 calibrator.getInitialBias(bias2);
7080 assertArrayEquals(bias1, bias2, 0.0);
7081 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7082 assertEquals(b1, ba);
7083 final Matrix b2 = new Matrix(3, 1);
7084 calibrator.getInitialBiasAsMatrix(b2);
7085 assertEquals(b1, b2);
7086 final Matrix ma1 = calibrator.getInitialMa();
7087 assertEquals(ma1, new Matrix(3, 3));
7088 final Matrix ma2 = new Matrix(3, 3);
7089 calibrator.getInitialMa(ma2);
7090 assertEquals(ma1, ma2);
7091 assertNull(calibrator.getEcefPosition());
7092 assertNull(calibrator.getNedPosition());
7093 assertFalse(calibrator.getNedPosition(null));
7094 assertNull(calibrator.getMeasurements());
7095 assertFalse(calibrator.isCommonAxisUsed());
7096 assertNull(calibrator.getListener());
7097 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7098 assertFalse(calibrator.isReady());
7099 assertFalse(calibrator.isRunning());
7100 assertNull(calibrator.getEstimatedBiases());
7101 assertFalse(calibrator.getEstimatedBiases(null));
7102 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7103 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7104 assertNull(calibrator.getEstimatedBiasFx());
7105 assertNull(calibrator.getEstimatedBiasFy());
7106 assertNull(calibrator.getEstimatedBiasFz());
7107 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7108 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7109 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7110 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7111 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7112 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7113 assertNull(calibrator.getEstimatedMa());
7114 assertNull(calibrator.getEstimatedSx());
7115 assertNull(calibrator.getEstimatedSy());
7116 assertNull(calibrator.getEstimatedSz());
7117 assertNull(calibrator.getEstimatedMxy());
7118 assertNull(calibrator.getEstimatedMxz());
7119 assertNull(calibrator.getEstimatedMyx());
7120 assertNull(calibrator.getEstimatedMyz());
7121 assertNull(calibrator.getEstimatedMzx());
7122 assertNull(calibrator.getEstimatedMzy());
7123 assertNull(calibrator.getEstimatedCovariance());
7124 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7125 assertNull(calibrator.getGroundTruthGravityNorm());
7126 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7127 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7128
7129
7130 calibrator = null;
7131 try {
7132 calibrator = new KnownPositionAccelerometerCalibrator(
7133 new Matrix(1, 1));
7134 fail("IllegalArgumentException expected but not thrown");
7135 } catch (final IllegalArgumentException ignore) {
7136 }
7137 try {
7138 calibrator = new KnownPositionAccelerometerCalibrator(
7139 new Matrix(1, 3));
7140 fail("IllegalArgumentException expected but not thrown");
7141 } catch (final IllegalArgumentException ignore) {
7142 }
7143 assertNull(calibrator);
7144 }
7145
7146 @Test
7147 public void testConstructor64() throws WrongSizeException {
7148 final Matrix ba = generateBa();
7149 final double biasX = ba.getElementAtIndex(0);
7150 final double biasY = ba.getElementAtIndex(1);
7151 final double biasZ = ba.getElementAtIndex(2);
7152
7153 KnownPositionAccelerometerCalibrator calibrator =
7154 new KnownPositionAccelerometerCalibrator(ba, this);
7155
7156
7157 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7158 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7159 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7160 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7161 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7162 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7163 final Acceleration bx2 = new Acceleration(0.0,
7164 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7165 calibrator.getInitialBiasXAsAcceleration(bx2);
7166 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7167 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7168 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7169 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7170 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7171 final Acceleration by2 = new Acceleration(0.0,
7172 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7173 calibrator.getInitialBiasYAsAcceleration(by2);
7174 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7175 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7176 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7177 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7178 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7179 final Acceleration bz2 = new Acceleration(0.0,
7180 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7181 calibrator.getInitialBiasZAsAcceleration(bz2);
7182 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7183 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7184 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7185 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7186 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7187 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7188 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7189 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7190 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7191 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7192 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7193 final double[] bias1 = calibrator.getInitialBias();
7194 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7195 final double[] bias2 = new double[3];
7196 calibrator.getInitialBias(bias2);
7197 assertArrayEquals(bias1, bias2, 0.0);
7198 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7199 assertEquals(b1, ba);
7200 final Matrix b2 = new Matrix(3, 1);
7201 calibrator.getInitialBiasAsMatrix(b2);
7202 assertEquals(b1, b2);
7203 final Matrix ma1 = calibrator.getInitialMa();
7204 assertEquals(ma1, new Matrix(3, 3));
7205 final Matrix ma2 = new Matrix(3, 3);
7206 calibrator.getInitialMa(ma2);
7207 assertEquals(ma1, ma2);
7208 assertNull(calibrator.getEcefPosition());
7209 assertNull(calibrator.getNedPosition());
7210 assertFalse(calibrator.getNedPosition(null));
7211 assertNull(calibrator.getMeasurements());
7212 assertFalse(calibrator.isCommonAxisUsed());
7213 assertSame(calibrator.getListener(), this);
7214 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7215 assertFalse(calibrator.isReady());
7216 assertFalse(calibrator.isRunning());
7217 assertNull(calibrator.getEstimatedBiases());
7218 assertFalse(calibrator.getEstimatedBiases(null));
7219 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7220 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7221 assertNull(calibrator.getEstimatedBiasFx());
7222 assertNull(calibrator.getEstimatedBiasFy());
7223 assertNull(calibrator.getEstimatedBiasFz());
7224 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7225 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7226 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7227 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7228 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7229 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7230 assertNull(calibrator.getEstimatedMa());
7231 assertNull(calibrator.getEstimatedSx());
7232 assertNull(calibrator.getEstimatedSy());
7233 assertNull(calibrator.getEstimatedSz());
7234 assertNull(calibrator.getEstimatedMxy());
7235 assertNull(calibrator.getEstimatedMxz());
7236 assertNull(calibrator.getEstimatedMyx());
7237 assertNull(calibrator.getEstimatedMyz());
7238 assertNull(calibrator.getEstimatedMzx());
7239 assertNull(calibrator.getEstimatedMzy());
7240 assertNull(calibrator.getEstimatedCovariance());
7241 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7242 assertNull(calibrator.getGroundTruthGravityNorm());
7243 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7244 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7245
7246
7247 calibrator = null;
7248 try {
7249 calibrator = new KnownPositionAccelerometerCalibrator(
7250 new Matrix(1, 1));
7251 fail("IllegalArgumentException expected but not thrown");
7252 } catch (final IllegalArgumentException ignore) {
7253 }
7254 try {
7255 calibrator = new KnownPositionAccelerometerCalibrator(
7256 new Matrix(1, 3));
7257 fail("IllegalArgumentException expected but not thrown");
7258 } catch (final IllegalArgumentException ignore) {
7259 }
7260 assertNull(calibrator);
7261 }
7262
7263 @Test
7264 public void testConstructor65() throws WrongSizeException {
7265 final Collection<StandardDeviationBodyKinematics> measurements =
7266 Collections.emptyList();
7267
7268 final Matrix ba = generateBa();
7269 final double biasX = ba.getElementAtIndex(0);
7270 final double biasY = ba.getElementAtIndex(1);
7271 final double biasZ = ba.getElementAtIndex(2);
7272
7273 KnownPositionAccelerometerCalibrator calibrator =
7274 new KnownPositionAccelerometerCalibrator(measurements, ba);
7275
7276
7277 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7278 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7279 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7280 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7281 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7282 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7283 final Acceleration bx2 = new Acceleration(0.0,
7284 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7285 calibrator.getInitialBiasXAsAcceleration(bx2);
7286 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7287 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7288 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7289 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7290 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7291 final Acceleration by2 = new Acceleration(0.0,
7292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7293 calibrator.getInitialBiasYAsAcceleration(by2);
7294 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7295 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7296 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7297 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7298 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7299 final Acceleration bz2 = new Acceleration(0.0,
7300 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7301 calibrator.getInitialBiasZAsAcceleration(bz2);
7302 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7303 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7304 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7305 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7306 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7307 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7308 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7309 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7310 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7311 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7312 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7313 final double[] bias1 = calibrator.getInitialBias();
7314 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7315 final double[] bias2 = new double[3];
7316 calibrator.getInitialBias(bias2);
7317 assertArrayEquals(bias1, bias2, 0.0);
7318 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7319 assertEquals(b1, ba);
7320 final Matrix b2 = new Matrix(3, 1);
7321 calibrator.getInitialBiasAsMatrix(b2);
7322 assertEquals(b1, b2);
7323 final Matrix ma1 = calibrator.getInitialMa();
7324 assertEquals(ma1, new Matrix(3, 3));
7325 final Matrix ma2 = new Matrix(3, 3);
7326 calibrator.getInitialMa(ma2);
7327 assertEquals(ma1, ma2);
7328 assertNull(calibrator.getEcefPosition());
7329 assertNull(calibrator.getNedPosition());
7330 assertFalse(calibrator.getNedPosition(null));
7331 assertSame(calibrator.getMeasurements(), measurements);
7332 assertFalse(calibrator.isCommonAxisUsed());
7333 assertNull(calibrator.getListener());
7334 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7335 assertFalse(calibrator.isReady());
7336 assertFalse(calibrator.isRunning());
7337 assertNull(calibrator.getEstimatedBiases());
7338 assertFalse(calibrator.getEstimatedBiases(null));
7339 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7340 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7341 assertNull(calibrator.getEstimatedBiasFx());
7342 assertNull(calibrator.getEstimatedBiasFy());
7343 assertNull(calibrator.getEstimatedBiasFz());
7344 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7345 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7346 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7347 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7348 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7349 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7350 assertNull(calibrator.getEstimatedMa());
7351 assertNull(calibrator.getEstimatedSx());
7352 assertNull(calibrator.getEstimatedSy());
7353 assertNull(calibrator.getEstimatedSz());
7354 assertNull(calibrator.getEstimatedMxy());
7355 assertNull(calibrator.getEstimatedMxz());
7356 assertNull(calibrator.getEstimatedMyx());
7357 assertNull(calibrator.getEstimatedMyz());
7358 assertNull(calibrator.getEstimatedMzx());
7359 assertNull(calibrator.getEstimatedMzy());
7360 assertNull(calibrator.getEstimatedCovariance());
7361 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7362 assertNull(calibrator.getGroundTruthGravityNorm());
7363 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7364 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7365
7366
7367 calibrator = null;
7368 try {
7369 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7370 new Matrix(1, 1));
7371 fail("IllegalArgumentException expected but not thrown");
7372 } catch (final IllegalArgumentException ignore) {
7373 }
7374 try {
7375 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7376 new Matrix(1, 3));
7377 fail("IllegalArgumentException expected but not thrown");
7378 } catch (final IllegalArgumentException ignore) {
7379 }
7380 assertNull(calibrator);
7381 }
7382
7383 @Test
7384 public void testConstructor66() throws WrongSizeException {
7385 final Collection<StandardDeviationBodyKinematics> measurements =
7386 Collections.emptyList();
7387
7388 final Matrix ba = generateBa();
7389 final double biasX = ba.getElementAtIndex(0);
7390 final double biasY = ba.getElementAtIndex(1);
7391 final double biasZ = ba.getElementAtIndex(2);
7392
7393 KnownPositionAccelerometerCalibrator calibrator =
7394 new KnownPositionAccelerometerCalibrator(measurements, ba,
7395 this);
7396
7397
7398 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7399 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7400 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7401 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7402 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7403 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7404 final Acceleration bx2 = new Acceleration(0.0,
7405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7406 calibrator.getInitialBiasXAsAcceleration(bx2);
7407 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7408 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7409 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7410 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7411 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7412 final Acceleration by2 = new Acceleration(0.0,
7413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7414 calibrator.getInitialBiasYAsAcceleration(by2);
7415 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7416 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7417 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7418 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7419 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7420 final Acceleration bz2 = new Acceleration(0.0,
7421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7422 calibrator.getInitialBiasZAsAcceleration(bz2);
7423 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7424 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7425 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7426 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7427 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7428 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7429 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7430 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7431 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7432 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7433 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7434 final double[] bias1 = calibrator.getInitialBias();
7435 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7436 final double[] bias2 = new double[3];
7437 calibrator.getInitialBias(bias2);
7438 assertArrayEquals(bias1, bias2, 0.0);
7439 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7440 assertEquals(b1, ba);
7441 final Matrix b2 = new Matrix(3, 1);
7442 calibrator.getInitialBiasAsMatrix(b2);
7443 assertEquals(b1, b2);
7444 final Matrix ma1 = calibrator.getInitialMa();
7445 assertEquals(ma1, new Matrix(3, 3));
7446 final Matrix ma2 = new Matrix(3, 3);
7447 calibrator.getInitialMa(ma2);
7448 assertEquals(ma1, ma2);
7449 assertNull(calibrator.getEcefPosition());
7450 assertNull(calibrator.getNedPosition());
7451 assertFalse(calibrator.getNedPosition(null));
7452 assertSame(calibrator.getMeasurements(), measurements);
7453 assertFalse(calibrator.isCommonAxisUsed());
7454 assertSame(calibrator.getListener(), this);
7455 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
7456 assertFalse(calibrator.isReady());
7457 assertFalse(calibrator.isRunning());
7458 assertNull(calibrator.getEstimatedBiases());
7459 assertFalse(calibrator.getEstimatedBiases(null));
7460 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7461 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7462 assertNull(calibrator.getEstimatedBiasFx());
7463 assertNull(calibrator.getEstimatedBiasFy());
7464 assertNull(calibrator.getEstimatedBiasFz());
7465 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7466 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7467 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7468 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7469 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7470 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7471 assertNull(calibrator.getEstimatedMa());
7472 assertNull(calibrator.getEstimatedSx());
7473 assertNull(calibrator.getEstimatedSy());
7474 assertNull(calibrator.getEstimatedSz());
7475 assertNull(calibrator.getEstimatedMxy());
7476 assertNull(calibrator.getEstimatedMxz());
7477 assertNull(calibrator.getEstimatedMyx());
7478 assertNull(calibrator.getEstimatedMyz());
7479 assertNull(calibrator.getEstimatedMzx());
7480 assertNull(calibrator.getEstimatedMzy());
7481 assertNull(calibrator.getEstimatedCovariance());
7482 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7483 assertNull(calibrator.getGroundTruthGravityNorm());
7484 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7485 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7486
7487
7488 calibrator = null;
7489 try {
7490 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7491 new Matrix(1, 1), this);
7492 fail("IllegalArgumentException expected but not thrown");
7493 } catch (final IllegalArgumentException ignore) {
7494 }
7495 try {
7496 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7497 new Matrix(1, 3), this);
7498 fail("IllegalArgumentException expected but not thrown");
7499 } catch (final IllegalArgumentException ignore) {
7500 }
7501 assertNull(calibrator);
7502 }
7503
7504 @Test
7505 public void testConstructor67() throws WrongSizeException {
7506 final Matrix ba = generateBa();
7507 final double biasX = ba.getElementAtIndex(0);
7508 final double biasY = ba.getElementAtIndex(1);
7509 final double biasZ = ba.getElementAtIndex(2);
7510
7511 KnownPositionAccelerometerCalibrator calibrator =
7512 new KnownPositionAccelerometerCalibrator(true, ba);
7513
7514
7515 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7516 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7517 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7518 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7519 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7520 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7521 final Acceleration bx2 = new Acceleration(0.0,
7522 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7523 calibrator.getInitialBiasXAsAcceleration(bx2);
7524 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7525 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7526 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7527 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7528 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7529 final Acceleration by2 = new Acceleration(0.0,
7530 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7531 calibrator.getInitialBiasYAsAcceleration(by2);
7532 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7533 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7534 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7535 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7536 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7537 final Acceleration bz2 = new Acceleration(0.0,
7538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7539 calibrator.getInitialBiasZAsAcceleration(bz2);
7540 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7541 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7542 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7543 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7544 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7545 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7546 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7547 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7548 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7549 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7550 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7551 final double[] bias1 = calibrator.getInitialBias();
7552 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7553 final double[] bias2 = new double[3];
7554 calibrator.getInitialBias(bias2);
7555 assertArrayEquals(bias1, bias2, 0.0);
7556 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7557 assertEquals(b1, ba);
7558 final Matrix b2 = new Matrix(3, 1);
7559 calibrator.getInitialBiasAsMatrix(b2);
7560 assertEquals(b1, b2);
7561 final Matrix ma1 = calibrator.getInitialMa();
7562 assertEquals(ma1, new Matrix(3, 3));
7563 final Matrix ma2 = new Matrix(3, 3);
7564 calibrator.getInitialMa(ma2);
7565 assertEquals(ma1, ma2);
7566 assertNull(calibrator.getEcefPosition());
7567 assertNull(calibrator.getNedPosition());
7568 assertFalse(calibrator.getNedPosition(null));
7569 assertNull(calibrator.getMeasurements());
7570 assertTrue(calibrator.isCommonAxisUsed());
7571 assertNull(calibrator.getListener());
7572 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7573 assertFalse(calibrator.isReady());
7574 assertFalse(calibrator.isRunning());
7575 assertNull(calibrator.getEstimatedBiases());
7576 assertFalse(calibrator.getEstimatedBiases(null));
7577 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7578 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7579 assertNull(calibrator.getEstimatedBiasFx());
7580 assertNull(calibrator.getEstimatedBiasFy());
7581 assertNull(calibrator.getEstimatedBiasFz());
7582 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7583 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7584 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7585 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7586 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7587 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7588 assertNull(calibrator.getEstimatedMa());
7589 assertNull(calibrator.getEstimatedSx());
7590 assertNull(calibrator.getEstimatedSy());
7591 assertNull(calibrator.getEstimatedSz());
7592 assertNull(calibrator.getEstimatedMxy());
7593 assertNull(calibrator.getEstimatedMxz());
7594 assertNull(calibrator.getEstimatedMyx());
7595 assertNull(calibrator.getEstimatedMyz());
7596 assertNull(calibrator.getEstimatedMzx());
7597 assertNull(calibrator.getEstimatedMzy());
7598 assertNull(calibrator.getEstimatedCovariance());
7599 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7600 assertNull(calibrator.getGroundTruthGravityNorm());
7601 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7602 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7603
7604
7605 calibrator = null;
7606 try {
7607 calibrator = new KnownPositionAccelerometerCalibrator(
7608 true, new Matrix(1, 1));
7609 fail("IllegalArgumentException expected but not thrown");
7610 } catch (final IllegalArgumentException ignore) {
7611 }
7612 try {
7613 calibrator = new KnownPositionAccelerometerCalibrator(
7614 true, new Matrix(1, 3));
7615 fail("IllegalArgumentException expected but not thrown");
7616 } catch (final IllegalArgumentException ignore) {
7617 }
7618 assertNull(calibrator);
7619 }
7620
7621 @Test
7622 public void testConstructor68() throws WrongSizeException {
7623 final Matrix ba = generateBa();
7624 final double biasX = ba.getElementAtIndex(0);
7625 final double biasY = ba.getElementAtIndex(1);
7626 final double biasZ = ba.getElementAtIndex(2);
7627
7628 KnownPositionAccelerometerCalibrator calibrator =
7629 new KnownPositionAccelerometerCalibrator(true, ba,
7630 this);
7631
7632
7633 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7634 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7635 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7636 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7637 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7638 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7639 final Acceleration bx2 = new Acceleration(0.0,
7640 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7641 calibrator.getInitialBiasXAsAcceleration(bx2);
7642 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7643 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7644 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7645 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7646 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7647 final Acceleration by2 = new Acceleration(0.0,
7648 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7649 calibrator.getInitialBiasYAsAcceleration(by2);
7650 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7651 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7652 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7653 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7654 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7655 final Acceleration bz2 = new Acceleration(0.0,
7656 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7657 calibrator.getInitialBiasZAsAcceleration(bz2);
7658 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7659 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7660 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7661 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7662 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7663 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7664 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7665 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7666 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7667 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7668 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7669 final double[] bias1 = calibrator.getInitialBias();
7670 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7671 final double[] bias2 = new double[3];
7672 calibrator.getInitialBias(bias2);
7673 assertArrayEquals(bias1, bias2, 0.0);
7674 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7675 assertEquals(b1, ba);
7676 final Matrix b2 = new Matrix(3, 1);
7677 calibrator.getInitialBiasAsMatrix(b2);
7678 assertEquals(b1, b2);
7679 final Matrix ma1 = calibrator.getInitialMa();
7680 assertEquals(ma1, new Matrix(3, 3));
7681 final Matrix ma2 = new Matrix(3, 3);
7682 calibrator.getInitialMa(ma2);
7683 assertEquals(ma1, ma2);
7684 assertNull(calibrator.getEcefPosition());
7685 assertNull(calibrator.getNedPosition());
7686 assertFalse(calibrator.getNedPosition(null));
7687 assertNull(calibrator.getMeasurements());
7688 assertTrue(calibrator.isCommonAxisUsed());
7689 assertSame(calibrator.getListener(), this);
7690 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7691 assertFalse(calibrator.isReady());
7692 assertFalse(calibrator.isRunning());
7693 assertNull(calibrator.getEstimatedBiases());
7694 assertFalse(calibrator.getEstimatedBiases(null));
7695 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7696 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7697 assertNull(calibrator.getEstimatedBiasFx());
7698 assertNull(calibrator.getEstimatedBiasFy());
7699 assertNull(calibrator.getEstimatedBiasFz());
7700 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7701 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7702 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7703 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7704 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7705 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7706 assertNull(calibrator.getEstimatedMa());
7707 assertNull(calibrator.getEstimatedSx());
7708 assertNull(calibrator.getEstimatedSy());
7709 assertNull(calibrator.getEstimatedSz());
7710 assertNull(calibrator.getEstimatedMxy());
7711 assertNull(calibrator.getEstimatedMxz());
7712 assertNull(calibrator.getEstimatedMyx());
7713 assertNull(calibrator.getEstimatedMyz());
7714 assertNull(calibrator.getEstimatedMzx());
7715 assertNull(calibrator.getEstimatedMzy());
7716 assertNull(calibrator.getEstimatedCovariance());
7717 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7718 assertNull(calibrator.getGroundTruthGravityNorm());
7719 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7720 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7721
7722
7723 calibrator = null;
7724 try {
7725 calibrator = new KnownPositionAccelerometerCalibrator(
7726 true, new Matrix(1, 1),
7727 this);
7728 fail("IllegalArgumentException expected but not thrown");
7729 } catch (final IllegalArgumentException ignore) {
7730 }
7731 try {
7732 calibrator = new KnownPositionAccelerometerCalibrator(
7733 true, new Matrix(1, 3),
7734 this);
7735 fail("IllegalArgumentException expected but not thrown");
7736 } catch (final IllegalArgumentException ignore) {
7737 }
7738 assertNull(calibrator);
7739 }
7740
7741 @Test
7742 public void testConstructor69() throws WrongSizeException {
7743 final Collection<StandardDeviationBodyKinematics> measurements =
7744 Collections.emptyList();
7745
7746 final Matrix ba = generateBa();
7747 final double biasX = ba.getElementAtIndex(0);
7748 final double biasY = ba.getElementAtIndex(1);
7749 final double biasZ = ba.getElementAtIndex(2);
7750
7751 KnownPositionAccelerometerCalibrator calibrator =
7752 new KnownPositionAccelerometerCalibrator(measurements,
7753 true, ba);
7754
7755
7756 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7757 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7758 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7759 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7760 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7761 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7762 final Acceleration bx2 = new Acceleration(0.0,
7763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7764 calibrator.getInitialBiasXAsAcceleration(bx2);
7765 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7766 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7767 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7768 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7769 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7770 final Acceleration by2 = new Acceleration(0.0,
7771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7772 calibrator.getInitialBiasYAsAcceleration(by2);
7773 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7774 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7775 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7776 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7777 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7778 final Acceleration bz2 = new Acceleration(0.0,
7779 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7780 calibrator.getInitialBiasZAsAcceleration(bz2);
7781 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7782 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7783 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7784 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7785 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7786 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7787 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7788 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7789 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7790 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7791 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7792 final double[] bias1 = calibrator.getInitialBias();
7793 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7794 final double[] bias2 = new double[3];
7795 calibrator.getInitialBias(bias2);
7796 assertArrayEquals(bias1, bias2, 0.0);
7797 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7798 assertEquals(b1, ba);
7799 final Matrix b2 = new Matrix(3, 1);
7800 calibrator.getInitialBiasAsMatrix(b2);
7801 assertEquals(b1, b2);
7802 final Matrix ma1 = calibrator.getInitialMa();
7803 assertEquals(ma1, new Matrix(3, 3));
7804 final Matrix ma2 = new Matrix(3, 3);
7805 calibrator.getInitialMa(ma2);
7806 assertEquals(ma1, ma2);
7807 assertNull(calibrator.getEcefPosition());
7808 assertNull(calibrator.getNedPosition());
7809 assertFalse(calibrator.getNedPosition(null));
7810 assertSame(calibrator.getMeasurements(), measurements);
7811 assertTrue(calibrator.isCommonAxisUsed());
7812 assertNull(calibrator.getListener());
7813 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7814 assertFalse(calibrator.isReady());
7815 assertFalse(calibrator.isRunning());
7816 assertNull(calibrator.getEstimatedBiases());
7817 assertFalse(calibrator.getEstimatedBiases(null));
7818 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7819 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7820 assertNull(calibrator.getEstimatedBiasFx());
7821 assertNull(calibrator.getEstimatedBiasFy());
7822 assertNull(calibrator.getEstimatedBiasFz());
7823 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7824 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7825 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7826 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7827 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7828 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7829 assertNull(calibrator.getEstimatedMa());
7830 assertNull(calibrator.getEstimatedSx());
7831 assertNull(calibrator.getEstimatedSy());
7832 assertNull(calibrator.getEstimatedSz());
7833 assertNull(calibrator.getEstimatedMxy());
7834 assertNull(calibrator.getEstimatedMxz());
7835 assertNull(calibrator.getEstimatedMyx());
7836 assertNull(calibrator.getEstimatedMyz());
7837 assertNull(calibrator.getEstimatedMzx());
7838 assertNull(calibrator.getEstimatedMzy());
7839 assertNull(calibrator.getEstimatedCovariance());
7840 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7841 assertNull(calibrator.getGroundTruthGravityNorm());
7842 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7843 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7844
7845
7846 calibrator = null;
7847 try {
7848 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7849 true, new Matrix(1, 1));
7850 fail("IllegalArgumentException expected but not thrown");
7851 } catch (final IllegalArgumentException ignore) {
7852 }
7853 try {
7854 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7855 true, new Matrix(1, 3));
7856 fail("IllegalArgumentException expected but not thrown");
7857 } catch (final IllegalArgumentException ignore) {
7858 }
7859 assertNull(calibrator);
7860 }
7861
7862 @Test
7863 public void testConstructor70() throws WrongSizeException {
7864 final Collection<StandardDeviationBodyKinematics> measurements =
7865 Collections.emptyList();
7866
7867 final Matrix ba = generateBa();
7868 final double biasX = ba.getElementAtIndex(0);
7869 final double biasY = ba.getElementAtIndex(1);
7870 final double biasZ = ba.getElementAtIndex(2);
7871
7872 KnownPositionAccelerometerCalibrator calibrator =
7873 new KnownPositionAccelerometerCalibrator(measurements,
7874 true, ba, this);
7875
7876
7877 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
7878 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
7879 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
7880 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
7881 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
7882 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7883 final Acceleration bx2 = new Acceleration(0.0,
7884 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7885 calibrator.getInitialBiasXAsAcceleration(bx2);
7886 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
7887 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7888 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
7889 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
7890 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7891 final Acceleration by2 = new Acceleration(0.0,
7892 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7893 calibrator.getInitialBiasYAsAcceleration(by2);
7894 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
7895 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7896 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
7897 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
7898 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7899 final Acceleration bz2 = new Acceleration(0.0,
7900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
7901 calibrator.getInitialBiasZAsAcceleration(bz2);
7902 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
7903 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
7904 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
7905 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
7906 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
7907 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
7908 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
7909 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
7910 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
7911 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
7912 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
7913 final double[] bias1 = calibrator.getInitialBias();
7914 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
7915 final double[] bias2 = new double[3];
7916 calibrator.getInitialBias(bias2);
7917 assertArrayEquals(bias1, bias2, 0.0);
7918 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
7919 assertEquals(b1, ba);
7920 final Matrix b2 = new Matrix(3, 1);
7921 calibrator.getInitialBiasAsMatrix(b2);
7922 assertEquals(b1, b2);
7923 final Matrix ma1 = calibrator.getInitialMa();
7924 assertEquals(ma1, new Matrix(3, 3));
7925 final Matrix ma2 = new Matrix(3, 3);
7926 calibrator.getInitialMa(ma2);
7927 assertEquals(ma1, ma2);
7928 assertNull(calibrator.getEcefPosition());
7929 assertNull(calibrator.getNedPosition());
7930 assertFalse(calibrator.getNedPosition(null));
7931 assertSame(calibrator.getMeasurements(), measurements);
7932 assertTrue(calibrator.isCommonAxisUsed());
7933 assertSame(calibrator.getListener(), this);
7934 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
7935 assertFalse(calibrator.isReady());
7936 assertFalse(calibrator.isRunning());
7937 assertNull(calibrator.getEstimatedBiases());
7938 assertFalse(calibrator.getEstimatedBiases(null));
7939 assertNull(calibrator.getEstimatedBiasesAsMatrix());
7940 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
7941 assertNull(calibrator.getEstimatedBiasFx());
7942 assertNull(calibrator.getEstimatedBiasFy());
7943 assertNull(calibrator.getEstimatedBiasFz());
7944 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
7945 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
7946 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
7947 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
7948 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
7949 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
7950 assertNull(calibrator.getEstimatedMa());
7951 assertNull(calibrator.getEstimatedSx());
7952 assertNull(calibrator.getEstimatedSy());
7953 assertNull(calibrator.getEstimatedSz());
7954 assertNull(calibrator.getEstimatedMxy());
7955 assertNull(calibrator.getEstimatedMxz());
7956 assertNull(calibrator.getEstimatedMyx());
7957 assertNull(calibrator.getEstimatedMyz());
7958 assertNull(calibrator.getEstimatedMzx());
7959 assertNull(calibrator.getEstimatedMzy());
7960 assertNull(calibrator.getEstimatedCovariance());
7961 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
7962 assertNull(calibrator.getGroundTruthGravityNorm());
7963 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
7964 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
7965
7966
7967 calibrator = null;
7968 try {
7969 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7970 true, new Matrix(1, 1),
7971 this);
7972 fail("IllegalArgumentException expected but not thrown");
7973 } catch (final IllegalArgumentException ignore) {
7974 }
7975 try {
7976 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
7977 true, new Matrix(1, 3),
7978 this);
7979 fail("IllegalArgumentException expected but not thrown");
7980 } catch (final IllegalArgumentException ignore) {
7981 }
7982 assertNull(calibrator);
7983 }
7984
7985 @Test
7986 public void testConstructor71() throws WrongSizeException {
7987 final Matrix ba = generateBa();
7988 final double biasX = ba.getElementAtIndex(0);
7989 final double biasY = ba.getElementAtIndex(1);
7990 final double biasZ = ba.getElementAtIndex(2);
7991
7992 final Matrix ma = generateMaCommonAxis();
7993 final double sx = ma.getElementAt(0, 0);
7994 final double sy = ma.getElementAt(1, 1);
7995 final double sz = ma.getElementAt(2, 2);
7996 final double mxy = ma.getElementAt(0, 1);
7997 final double mxz = ma.getElementAt(0, 2);
7998 final double myx = ma.getElementAt(1, 0);
7999 final double myz = ma.getElementAt(1, 2);
8000 final double mzx = ma.getElementAt(2, 0);
8001 final double mzy = ma.getElementAt(2, 1);
8002
8003 KnownPositionAccelerometerCalibrator calibrator =
8004 new KnownPositionAccelerometerCalibrator(ba, ma);
8005
8006
8007 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8008 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8009 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8010 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8011 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8012 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8013 final Acceleration bx2 = new Acceleration(0.0,
8014 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8015 calibrator.getInitialBiasXAsAcceleration(bx2);
8016 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8017 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8018 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8019 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8020 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8021 final Acceleration by2 = new Acceleration(0.0,
8022 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8023 calibrator.getInitialBiasYAsAcceleration(by2);
8024 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8025 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8026 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8027 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8028 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8029 final Acceleration bz2 = new Acceleration(0.0,
8030 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8031 calibrator.getInitialBiasZAsAcceleration(bz2);
8032 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8033 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8034 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8035 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8036 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8037 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8038 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8039 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8040 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8041 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8042 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8043 final double[] bias1 = calibrator.getInitialBias();
8044 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8045 final double[] bias2 = new double[3];
8046 calibrator.getInitialBias(bias2);
8047 assertArrayEquals(bias1, bias2, 0.0);
8048 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8049 assertEquals(b1, ba);
8050 final Matrix b2 = new Matrix(3, 1);
8051 calibrator.getInitialBiasAsMatrix(b2);
8052 assertEquals(b1, b2);
8053 final Matrix ma1 = new Matrix(3, 3);
8054 ma1.setSubmatrix(0, 0,
8055 2, 2,
8056 new double[]{sx, myx, mzx,
8057 mxy, sy, mzy,
8058 mxz, myz, sz});
8059 assertEquals(calibrator.getInitialMa(), ma1);
8060 final Matrix ma2 = new Matrix(3, 3);
8061 calibrator.getInitialMa(ma2);
8062 assertEquals(ma1, ma2);
8063 assertNull(calibrator.getEcefPosition());
8064 assertNull(calibrator.getNedPosition());
8065 assertFalse(calibrator.getNedPosition(null));
8066 assertNull(calibrator.getMeasurements());
8067 assertFalse(calibrator.isCommonAxisUsed());
8068 assertNull(calibrator.getListener());
8069 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8070 assertFalse(calibrator.isReady());
8071 assertFalse(calibrator.isRunning());
8072 assertNull(calibrator.getEstimatedBiases());
8073 assertFalse(calibrator.getEstimatedBiases(null));
8074 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8075 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8076 assertNull(calibrator.getEstimatedBiasFx());
8077 assertNull(calibrator.getEstimatedBiasFy());
8078 assertNull(calibrator.getEstimatedBiasFz());
8079 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8080 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8081 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8082 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8083 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8084 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8085 assertNull(calibrator.getEstimatedMa());
8086 assertNull(calibrator.getEstimatedSx());
8087 assertNull(calibrator.getEstimatedSy());
8088 assertNull(calibrator.getEstimatedSz());
8089 assertNull(calibrator.getEstimatedMxy());
8090 assertNull(calibrator.getEstimatedMxz());
8091 assertNull(calibrator.getEstimatedMyx());
8092 assertNull(calibrator.getEstimatedMyz());
8093 assertNull(calibrator.getEstimatedMzx());
8094 assertNull(calibrator.getEstimatedMzy());
8095 assertNull(calibrator.getEstimatedCovariance());
8096 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8097 assertNull(calibrator.getGroundTruthGravityNorm());
8098 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8099 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8100
8101
8102 calibrator = null;
8103 try {
8104 calibrator = new KnownPositionAccelerometerCalibrator(
8105 new Matrix(1, 1), ma);
8106 fail("IllegalArgumentException expected but not thrown");
8107 } catch (final IllegalArgumentException ignore) {
8108 }
8109 try {
8110 calibrator = new KnownPositionAccelerometerCalibrator(
8111 new Matrix(1, 3), ma);
8112 fail("IllegalArgumentException expected but not thrown");
8113 } catch (final IllegalArgumentException ignore) {
8114 }
8115 try {
8116 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8117 new Matrix(1, 3));
8118 fail("IllegalArgumentException expected but not thrown");
8119 } catch (final IllegalArgumentException ignore) {
8120 }
8121 try {
8122 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8123 new Matrix(3, 1));
8124 fail("IllegalArgumentException expected but not thrown");
8125 } catch (final IllegalArgumentException ignore) {
8126 }
8127 assertNull(calibrator);
8128 }
8129
8130 @Test
8131 public void testConstructor72() throws WrongSizeException {
8132 final Matrix ba = generateBa();
8133 final double biasX = ba.getElementAtIndex(0);
8134 final double biasY = ba.getElementAtIndex(1);
8135 final double biasZ = ba.getElementAtIndex(2);
8136
8137 final Matrix ma = generateMaCommonAxis();
8138 final double sx = ma.getElementAt(0, 0);
8139 final double sy = ma.getElementAt(1, 1);
8140 final double sz = ma.getElementAt(2, 2);
8141 final double mxy = ma.getElementAt(0, 1);
8142 final double mxz = ma.getElementAt(0, 2);
8143 final double myx = ma.getElementAt(1, 0);
8144 final double myz = ma.getElementAt(1, 2);
8145 final double mzx = ma.getElementAt(2, 0);
8146 final double mzy = ma.getElementAt(2, 1);
8147
8148 KnownPositionAccelerometerCalibrator calibrator =
8149 new KnownPositionAccelerometerCalibrator(ba, ma, this);
8150
8151
8152 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8153 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8154 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8155 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8156 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8157 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8158 final Acceleration bx2 = new Acceleration(0.0,
8159 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8160 calibrator.getInitialBiasXAsAcceleration(bx2);
8161 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8162 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8163 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8164 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8165 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8166 final Acceleration by2 = new Acceleration(0.0,
8167 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8168 calibrator.getInitialBiasYAsAcceleration(by2);
8169 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8170 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8171 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8172 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8173 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8174 final Acceleration bz2 = new Acceleration(0.0,
8175 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8176 calibrator.getInitialBiasZAsAcceleration(bz2);
8177 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8178 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8179 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8180 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8181 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8182 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8183 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8184 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8185 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8186 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8187 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8188 final double[] bias1 = calibrator.getInitialBias();
8189 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8190 final double[] bias2 = new double[3];
8191 calibrator.getInitialBias(bias2);
8192 assertArrayEquals(bias1, bias2, 0.0);
8193 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8194 assertEquals(b1, ba);
8195 final Matrix b2 = new Matrix(3, 1);
8196 calibrator.getInitialBiasAsMatrix(b2);
8197 assertEquals(b1, b2);
8198 final Matrix ma1 = new Matrix(3, 3);
8199 ma1.setSubmatrix(0, 0,
8200 2, 2,
8201 new double[]{sx, myx, mzx,
8202 mxy, sy, mzy,
8203 mxz, myz, sz});
8204 assertEquals(calibrator.getInitialMa(), ma1);
8205 final Matrix ma2 = new Matrix(3, 3);
8206 calibrator.getInitialMa(ma2);
8207 assertEquals(ma1, ma2);
8208 assertNull(calibrator.getEcefPosition());
8209 assertNull(calibrator.getNedPosition());
8210 assertFalse(calibrator.getNedPosition(null));
8211 assertNull(calibrator.getMeasurements());
8212 assertFalse(calibrator.isCommonAxisUsed());
8213 assertSame(calibrator.getListener(), this);
8214 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8215 assertFalse(calibrator.isReady());
8216 assertFalse(calibrator.isRunning());
8217 assertNull(calibrator.getEstimatedBiases());
8218 assertFalse(calibrator.getEstimatedBiases(null));
8219 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8220 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8221 assertNull(calibrator.getEstimatedBiasFx());
8222 assertNull(calibrator.getEstimatedBiasFy());
8223 assertNull(calibrator.getEstimatedBiasFz());
8224 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8225 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8226 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8227 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8228 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8229 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8230 assertNull(calibrator.getEstimatedMa());
8231 assertNull(calibrator.getEstimatedSx());
8232 assertNull(calibrator.getEstimatedSy());
8233 assertNull(calibrator.getEstimatedSz());
8234 assertNull(calibrator.getEstimatedMxy());
8235 assertNull(calibrator.getEstimatedMxz());
8236 assertNull(calibrator.getEstimatedMyx());
8237 assertNull(calibrator.getEstimatedMyz());
8238 assertNull(calibrator.getEstimatedMzx());
8239 assertNull(calibrator.getEstimatedMzy());
8240 assertNull(calibrator.getEstimatedCovariance());
8241 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8242 assertNull(calibrator.getGroundTruthGravityNorm());
8243 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8244 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8245
8246
8247 calibrator = null;
8248 try {
8249 calibrator = new KnownPositionAccelerometerCalibrator(
8250 new Matrix(1, 1), ma, this);
8251 fail("IllegalArgumentException expected but not thrown");
8252 } catch (final IllegalArgumentException ignore) {
8253 }
8254 try {
8255 calibrator = new KnownPositionAccelerometerCalibrator(
8256 new Matrix(1, 3), ma, this);
8257 fail("IllegalArgumentException expected but not thrown");
8258 } catch (final IllegalArgumentException ignore) {
8259 }
8260 try {
8261 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8262 new Matrix(1, 3), this);
8263 fail("IllegalArgumentException expected but not thrown");
8264 } catch (final IllegalArgumentException ignore) {
8265 }
8266 try {
8267 calibrator = new KnownPositionAccelerometerCalibrator(ba,
8268 new Matrix(3, 1), this);
8269 fail("IllegalArgumentException expected but not thrown");
8270 } catch (final IllegalArgumentException ignore) {
8271 }
8272 assertNull(calibrator);
8273 }
8274
8275 @Test
8276 public void testConstructor73() throws WrongSizeException {
8277 final Collection<StandardDeviationBodyKinematics> measurements =
8278 Collections.emptyList();
8279
8280 final Matrix ba = generateBa();
8281 final double biasX = ba.getElementAtIndex(0);
8282 final double biasY = ba.getElementAtIndex(1);
8283 final double biasZ = ba.getElementAtIndex(2);
8284
8285 final Matrix ma = generateMaCommonAxis();
8286 final double sx = ma.getElementAt(0, 0);
8287 final double sy = ma.getElementAt(1, 1);
8288 final double sz = ma.getElementAt(2, 2);
8289 final double mxy = ma.getElementAt(0, 1);
8290 final double mxz = ma.getElementAt(0, 2);
8291 final double myx = ma.getElementAt(1, 0);
8292 final double myz = ma.getElementAt(1, 2);
8293 final double mzx = ma.getElementAt(2, 0);
8294 final double mzy = ma.getElementAt(2, 1);
8295
8296 KnownPositionAccelerometerCalibrator calibrator =
8297 new KnownPositionAccelerometerCalibrator(measurements, ba, ma);
8298
8299
8300 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8301 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8302 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8303 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8304 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8305 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8306 final Acceleration bx2 = new Acceleration(0.0,
8307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8308 calibrator.getInitialBiasXAsAcceleration(bx2);
8309 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8310 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8311 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8312 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8313 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8314 final Acceleration by2 = new Acceleration(0.0,
8315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8316 calibrator.getInitialBiasYAsAcceleration(by2);
8317 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8318 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8319 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8320 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8321 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8322 final Acceleration bz2 = new Acceleration(0.0,
8323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8324 calibrator.getInitialBiasZAsAcceleration(bz2);
8325 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8326 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8327 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8328 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8329 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8330 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8331 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8332 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8333 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8334 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8335 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8336 final double[] bias1 = calibrator.getInitialBias();
8337 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8338 final double[] bias2 = new double[3];
8339 calibrator.getInitialBias(bias2);
8340 assertArrayEquals(bias1, bias2, 0.0);
8341 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8342 assertEquals(b1, ba);
8343 final Matrix b2 = new Matrix(3, 1);
8344 calibrator.getInitialBiasAsMatrix(b2);
8345 assertEquals(b1, b2);
8346 final Matrix ma1 = new Matrix(3, 3);
8347 ma1.setSubmatrix(0, 0,
8348 2, 2,
8349 new double[]{sx, myx, mzx,
8350 mxy, sy, mzy,
8351 mxz, myz, sz});
8352 assertEquals(calibrator.getInitialMa(), ma1);
8353 final Matrix ma2 = new Matrix(3, 3);
8354 calibrator.getInitialMa(ma2);
8355 assertEquals(ma1, ma2);
8356 assertNull(calibrator.getEcefPosition());
8357 assertNull(calibrator.getNedPosition());
8358 assertFalse(calibrator.getNedPosition(null));
8359 assertSame(calibrator.getMeasurements(), measurements);
8360 assertFalse(calibrator.isCommonAxisUsed());
8361 assertNull(calibrator.getListener());
8362 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8363 assertFalse(calibrator.isReady());
8364 assertFalse(calibrator.isRunning());
8365 assertNull(calibrator.getEstimatedBiases());
8366 assertFalse(calibrator.getEstimatedBiases(null));
8367 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8368 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8369 assertNull(calibrator.getEstimatedBiasFx());
8370 assertNull(calibrator.getEstimatedBiasFy());
8371 assertNull(calibrator.getEstimatedBiasFz());
8372 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8373 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8374 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8375 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8376 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8377 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8378 assertNull(calibrator.getEstimatedMa());
8379 assertNull(calibrator.getEstimatedSx());
8380 assertNull(calibrator.getEstimatedSy());
8381 assertNull(calibrator.getEstimatedSz());
8382 assertNull(calibrator.getEstimatedMxy());
8383 assertNull(calibrator.getEstimatedMxz());
8384 assertNull(calibrator.getEstimatedMyx());
8385 assertNull(calibrator.getEstimatedMyz());
8386 assertNull(calibrator.getEstimatedMzx());
8387 assertNull(calibrator.getEstimatedMzy());
8388 assertNull(calibrator.getEstimatedCovariance());
8389 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8390 assertNull(calibrator.getGroundTruthGravityNorm());
8391 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8392 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8393
8394
8395 calibrator = null;
8396 try {
8397 calibrator = new KnownPositionAccelerometerCalibrator(
8398 measurements, new Matrix(1, 1), ma);
8399 fail("IllegalArgumentException expected but not thrown");
8400 } catch (final IllegalArgumentException ignore) {
8401 }
8402 try {
8403 calibrator = new KnownPositionAccelerometerCalibrator(
8404 measurements, new Matrix(1, 3), ma);
8405 fail("IllegalArgumentException expected but not thrown");
8406 } catch (final IllegalArgumentException ignore) {
8407 }
8408 try {
8409 calibrator = new KnownPositionAccelerometerCalibrator(
8410 measurements, ba, new Matrix(1, 3));
8411 fail("IllegalArgumentException expected but not thrown");
8412 } catch (final IllegalArgumentException ignore) {
8413 }
8414 try {
8415 calibrator = new KnownPositionAccelerometerCalibrator(
8416 measurements, ba, new Matrix(3, 1));
8417 fail("IllegalArgumentException expected but not thrown");
8418 } catch (final IllegalArgumentException ignore) {
8419 }
8420 assertNull(calibrator);
8421 }
8422
8423 @Test
8424 public void testConstructor74() throws WrongSizeException {
8425 final Collection<StandardDeviationBodyKinematics> measurements =
8426 Collections.emptyList();
8427
8428 final Matrix ba = generateBa();
8429 final double biasX = ba.getElementAtIndex(0);
8430 final double biasY = ba.getElementAtIndex(1);
8431 final double biasZ = ba.getElementAtIndex(2);
8432
8433 final Matrix ma = generateMaCommonAxis();
8434 final double sx = ma.getElementAt(0, 0);
8435 final double sy = ma.getElementAt(1, 1);
8436 final double sz = ma.getElementAt(2, 2);
8437 final double mxy = ma.getElementAt(0, 1);
8438 final double mxz = ma.getElementAt(0, 2);
8439 final double myx = ma.getElementAt(1, 0);
8440 final double myz = ma.getElementAt(1, 2);
8441 final double mzx = ma.getElementAt(2, 0);
8442 final double mzy = ma.getElementAt(2, 1);
8443
8444 KnownPositionAccelerometerCalibrator calibrator =
8445 new KnownPositionAccelerometerCalibrator(measurements, ba, ma,
8446 this);
8447
8448
8449 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8450 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8451 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8452 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8453 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8454 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8455 final Acceleration bx2 = new Acceleration(0.0,
8456 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8457 calibrator.getInitialBiasXAsAcceleration(bx2);
8458 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8459 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8460 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8461 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8462 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8463 final Acceleration by2 = new Acceleration(0.0,
8464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8465 calibrator.getInitialBiasYAsAcceleration(by2);
8466 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8467 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8468 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8469 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8470 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8471 final Acceleration bz2 = new Acceleration(0.0,
8472 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8473 calibrator.getInitialBiasZAsAcceleration(bz2);
8474 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8475 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8476 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8477 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8478 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8479 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8480 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8481 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8482 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8483 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8484 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8485 final double[] bias1 = calibrator.getInitialBias();
8486 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8487 final double[] bias2 = new double[3];
8488 calibrator.getInitialBias(bias2);
8489 assertArrayEquals(bias1, bias2, 0.0);
8490 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8491 assertEquals(b1, ba);
8492 final Matrix b2 = new Matrix(3, 1);
8493 calibrator.getInitialBiasAsMatrix(b2);
8494 assertEquals(b1, b2);
8495 final Matrix ma1 = new Matrix(3, 3);
8496 ma1.setSubmatrix(0, 0,
8497 2, 2,
8498 new double[]{sx, myx, mzx,
8499 mxy, sy, mzy,
8500 mxz, myz, sz});
8501 assertEquals(calibrator.getInitialMa(), ma1);
8502 final Matrix ma2 = new Matrix(3, 3);
8503 calibrator.getInitialMa(ma2);
8504 assertEquals(ma1, ma2);
8505 assertNull(calibrator.getEcefPosition());
8506 assertNull(calibrator.getNedPosition());
8507 assertFalse(calibrator.getNedPosition(null));
8508 assertSame(calibrator.getMeasurements(), measurements);
8509 assertFalse(calibrator.isCommonAxisUsed());
8510 assertSame(calibrator.getListener(), this);
8511 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
8512 assertFalse(calibrator.isReady());
8513 assertFalse(calibrator.isRunning());
8514 assertNull(calibrator.getEstimatedBiases());
8515 assertFalse(calibrator.getEstimatedBiases(null));
8516 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8517 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8518 assertNull(calibrator.getEstimatedBiasFx());
8519 assertNull(calibrator.getEstimatedBiasFy());
8520 assertNull(calibrator.getEstimatedBiasFz());
8521 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8522 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8523 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8524 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8525 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8526 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8527 assertNull(calibrator.getEstimatedMa());
8528 assertNull(calibrator.getEstimatedSx());
8529 assertNull(calibrator.getEstimatedSy());
8530 assertNull(calibrator.getEstimatedSz());
8531 assertNull(calibrator.getEstimatedMxy());
8532 assertNull(calibrator.getEstimatedMxz());
8533 assertNull(calibrator.getEstimatedMyx());
8534 assertNull(calibrator.getEstimatedMyz());
8535 assertNull(calibrator.getEstimatedMzx());
8536 assertNull(calibrator.getEstimatedMzy());
8537 assertNull(calibrator.getEstimatedCovariance());
8538 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8539 assertNull(calibrator.getGroundTruthGravityNorm());
8540 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8541 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8542
8543
8544 calibrator = null;
8545 try {
8546 calibrator = new KnownPositionAccelerometerCalibrator(
8547 measurements, new Matrix(1, 1), ma, this);
8548 fail("IllegalArgumentException expected but not thrown");
8549 } catch (final IllegalArgumentException ignore) {
8550 }
8551 try {
8552 calibrator = new KnownPositionAccelerometerCalibrator(
8553 measurements, new Matrix(1, 3), ma, this);
8554 fail("IllegalArgumentException expected but not thrown");
8555 } catch (final IllegalArgumentException ignore) {
8556 }
8557 try {
8558 calibrator = new KnownPositionAccelerometerCalibrator(
8559 measurements, ba, new Matrix(1, 3), this);
8560 fail("IllegalArgumentException expected but not thrown");
8561 } catch (final IllegalArgumentException ignore) {
8562 }
8563 try {
8564 calibrator = new KnownPositionAccelerometerCalibrator(
8565 measurements, ba, new Matrix(3, 1), this);
8566 fail("IllegalArgumentException expected but not thrown");
8567 } catch (final IllegalArgumentException ignore) {
8568 }
8569 assertNull(calibrator);
8570 }
8571
8572 @Test
8573 public void testConstructor75() throws WrongSizeException {
8574 final Matrix ba = generateBa();
8575 final double biasX = ba.getElementAtIndex(0);
8576 final double biasY = ba.getElementAtIndex(1);
8577 final double biasZ = ba.getElementAtIndex(2);
8578
8579 final Matrix ma = generateMaCommonAxis();
8580 final double sx = ma.getElementAt(0, 0);
8581 final double sy = ma.getElementAt(1, 1);
8582 final double sz = ma.getElementAt(2, 2);
8583 final double mxy = ma.getElementAt(0, 1);
8584 final double mxz = ma.getElementAt(0, 2);
8585 final double myx = ma.getElementAt(1, 0);
8586 final double myz = ma.getElementAt(1, 2);
8587 final double mzx = ma.getElementAt(2, 0);
8588 final double mzy = ma.getElementAt(2, 1);
8589
8590 KnownPositionAccelerometerCalibrator calibrator =
8591 new KnownPositionAccelerometerCalibrator(true, ba, ma);
8592
8593
8594 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8595 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8596 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8597 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8598 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8599 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8600 final Acceleration bx2 = new Acceleration(0.0,
8601 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8602 calibrator.getInitialBiasXAsAcceleration(bx2);
8603 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8604 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8605 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8606 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8607 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8608 final Acceleration by2 = new Acceleration(0.0,
8609 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8610 calibrator.getInitialBiasYAsAcceleration(by2);
8611 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8612 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8613 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8614 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8615 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8616 final Acceleration bz2 = new Acceleration(0.0,
8617 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8618 calibrator.getInitialBiasZAsAcceleration(bz2);
8619 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8620 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8621 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8622 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8623 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8624 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8625 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8626 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8627 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8628 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8629 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8630 final double[] bias1 = calibrator.getInitialBias();
8631 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8632 final double[] bias2 = new double[3];
8633 calibrator.getInitialBias(bias2);
8634 assertArrayEquals(bias1, bias2, 0.0);
8635 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8636 assertEquals(b1, ba);
8637 final Matrix b2 = new Matrix(3, 1);
8638 calibrator.getInitialBiasAsMatrix(b2);
8639 assertEquals(b1, b2);
8640 final Matrix ma1 = new Matrix(3, 3);
8641 ma1.setSubmatrix(0, 0,
8642 2, 2,
8643 new double[]{sx, myx, mzx,
8644 mxy, sy, mzy,
8645 mxz, myz, sz});
8646 assertEquals(calibrator.getInitialMa(), ma1);
8647 final Matrix ma2 = new Matrix(3, 3);
8648 calibrator.getInitialMa(ma2);
8649 assertEquals(ma1, ma2);
8650 assertNull(calibrator.getEcefPosition());
8651 assertNull(calibrator.getNedPosition());
8652 assertFalse(calibrator.getNedPosition(null));
8653 assertNull(calibrator.getMeasurements());
8654 assertTrue(calibrator.isCommonAxisUsed());
8655 assertNull(calibrator.getListener());
8656 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8657 assertFalse(calibrator.isReady());
8658 assertFalse(calibrator.isRunning());
8659 assertNull(calibrator.getEstimatedBiases());
8660 assertFalse(calibrator.getEstimatedBiases(null));
8661 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8662 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8663 assertNull(calibrator.getEstimatedBiasFx());
8664 assertNull(calibrator.getEstimatedBiasFy());
8665 assertNull(calibrator.getEstimatedBiasFz());
8666 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8667 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8668 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8669 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8670 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8671 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8672 assertNull(calibrator.getEstimatedMa());
8673 assertNull(calibrator.getEstimatedSx());
8674 assertNull(calibrator.getEstimatedSy());
8675 assertNull(calibrator.getEstimatedSz());
8676 assertNull(calibrator.getEstimatedMxy());
8677 assertNull(calibrator.getEstimatedMxz());
8678 assertNull(calibrator.getEstimatedMyx());
8679 assertNull(calibrator.getEstimatedMyz());
8680 assertNull(calibrator.getEstimatedMzx());
8681 assertNull(calibrator.getEstimatedMzy());
8682 assertNull(calibrator.getEstimatedCovariance());
8683 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8684 assertNull(calibrator.getGroundTruthGravityNorm());
8685 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8686 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8687
8688
8689 calibrator = null;
8690 try {
8691 calibrator = new KnownPositionAccelerometerCalibrator(
8692 true, new Matrix(1, 1), ma);
8693 fail("IllegalArgumentException expected but not thrown");
8694 } catch (final IllegalArgumentException ignore) {
8695 }
8696 try {
8697 calibrator = new KnownPositionAccelerometerCalibrator(
8698 true, new Matrix(1, 3), ma);
8699 fail("IllegalArgumentException expected but not thrown");
8700 } catch (final IllegalArgumentException ignore) {
8701 }
8702 try {
8703 calibrator = new KnownPositionAccelerometerCalibrator(
8704 true, ba, new Matrix(1, 3));
8705 fail("IllegalArgumentException expected but not thrown");
8706 } catch (final IllegalArgumentException ignore) {
8707 }
8708 try {
8709 calibrator = new KnownPositionAccelerometerCalibrator(
8710 true, ba, new Matrix(3, 1));
8711 fail("IllegalArgumentException expected but not thrown");
8712 } catch (final IllegalArgumentException ignore) {
8713 }
8714 assertNull(calibrator);
8715 }
8716
8717 @Test
8718 public void testConstructor76() throws WrongSizeException {
8719 final Matrix ba = generateBa();
8720 final double biasX = ba.getElementAtIndex(0);
8721 final double biasY = ba.getElementAtIndex(1);
8722 final double biasZ = ba.getElementAtIndex(2);
8723
8724 final Matrix ma = generateMaCommonAxis();
8725 final double sx = ma.getElementAt(0, 0);
8726 final double sy = ma.getElementAt(1, 1);
8727 final double sz = ma.getElementAt(2, 2);
8728 final double mxy = ma.getElementAt(0, 1);
8729 final double mxz = ma.getElementAt(0, 2);
8730 final double myx = ma.getElementAt(1, 0);
8731 final double myz = ma.getElementAt(1, 2);
8732 final double mzx = ma.getElementAt(2, 0);
8733 final double mzy = ma.getElementAt(2, 1);
8734
8735 KnownPositionAccelerometerCalibrator calibrator =
8736 new KnownPositionAccelerometerCalibrator(true, ba, ma,
8737 this);
8738
8739
8740 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8741 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8742 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8743 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8744 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8745 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8746 final Acceleration bx2 = new Acceleration(0.0,
8747 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8748 calibrator.getInitialBiasXAsAcceleration(bx2);
8749 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8750 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8751 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8752 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8753 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8754 final Acceleration by2 = new Acceleration(0.0,
8755 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8756 calibrator.getInitialBiasYAsAcceleration(by2);
8757 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8758 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8759 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8760 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8761 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8762 final Acceleration bz2 = new Acceleration(0.0,
8763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8764 calibrator.getInitialBiasZAsAcceleration(bz2);
8765 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8766 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8767 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8768 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8769 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8770 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8771 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8772 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8773 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8774 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8775 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8776 final double[] bias1 = calibrator.getInitialBias();
8777 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8778 final double[] bias2 = new double[3];
8779 calibrator.getInitialBias(bias2);
8780 assertArrayEquals(bias1, bias2, 0.0);
8781 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8782 assertEquals(b1, ba);
8783 final Matrix b2 = new Matrix(3, 1);
8784 calibrator.getInitialBiasAsMatrix(b2);
8785 assertEquals(b1, b2);
8786 final Matrix ma1 = new Matrix(3, 3);
8787 ma1.setSubmatrix(0, 0,
8788 2, 2,
8789 new double[]{sx, myx, mzx,
8790 mxy, sy, mzy,
8791 mxz, myz, sz});
8792 assertEquals(calibrator.getInitialMa(), ma1);
8793 final Matrix ma2 = new Matrix(3, 3);
8794 calibrator.getInitialMa(ma2);
8795 assertEquals(ma1, ma2);
8796 assertNull(calibrator.getEcefPosition());
8797 assertNull(calibrator.getNedPosition());
8798 assertFalse(calibrator.getNedPosition(null));
8799 assertNull(calibrator.getMeasurements());
8800 assertTrue(calibrator.isCommonAxisUsed());
8801 assertSame(calibrator.getListener(), this);
8802 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8803 assertFalse(calibrator.isReady());
8804 assertFalse(calibrator.isRunning());
8805 assertNull(calibrator.getEstimatedBiases());
8806 assertFalse(calibrator.getEstimatedBiases(null));
8807 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8808 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8809 assertNull(calibrator.getEstimatedBiasFx());
8810 assertNull(calibrator.getEstimatedBiasFy());
8811 assertNull(calibrator.getEstimatedBiasFz());
8812 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8813 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8814 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8815 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8816 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8817 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8818 assertNull(calibrator.getEstimatedMa());
8819 assertNull(calibrator.getEstimatedSx());
8820 assertNull(calibrator.getEstimatedSy());
8821 assertNull(calibrator.getEstimatedSz());
8822 assertNull(calibrator.getEstimatedMxy());
8823 assertNull(calibrator.getEstimatedMxz());
8824 assertNull(calibrator.getEstimatedMyx());
8825 assertNull(calibrator.getEstimatedMyz());
8826 assertNull(calibrator.getEstimatedMzx());
8827 assertNull(calibrator.getEstimatedMzy());
8828 assertNull(calibrator.getEstimatedCovariance());
8829 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8830 assertNull(calibrator.getGroundTruthGravityNorm());
8831 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8832 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8833
8834
8835 calibrator = null;
8836 try {
8837 calibrator = new KnownPositionAccelerometerCalibrator(
8838 true, new Matrix(1, 1), ma,
8839 this);
8840 fail("IllegalArgumentException expected but not thrown");
8841 } catch (final IllegalArgumentException ignore) {
8842 }
8843 try {
8844 calibrator = new KnownPositionAccelerometerCalibrator(
8845 true, new Matrix(1, 3), ma,
8846 this);
8847 fail("IllegalArgumentException expected but not thrown");
8848 } catch (final IllegalArgumentException ignore) {
8849 }
8850 try {
8851 calibrator = new KnownPositionAccelerometerCalibrator(
8852 true, ba, new Matrix(1, 3),
8853 this);
8854 fail("IllegalArgumentException expected but not thrown");
8855 } catch (final IllegalArgumentException ignore) {
8856 }
8857 try {
8858 calibrator = new KnownPositionAccelerometerCalibrator(
8859 true, ba, new Matrix(3, 1),
8860 this);
8861 fail("IllegalArgumentException expected but not thrown");
8862 } catch (final IllegalArgumentException ignore) {
8863 }
8864 assertNull(calibrator);
8865 }
8866
8867 @Test
8868 public void testConstructor77() throws WrongSizeException {
8869 final Collection<StandardDeviationBodyKinematics> measurements =
8870 Collections.emptyList();
8871
8872 final Matrix ba = generateBa();
8873 final double biasX = ba.getElementAtIndex(0);
8874 final double biasY = ba.getElementAtIndex(1);
8875 final double biasZ = ba.getElementAtIndex(2);
8876
8877 final Matrix ma = generateMaCommonAxis();
8878 final double sx = ma.getElementAt(0, 0);
8879 final double sy = ma.getElementAt(1, 1);
8880 final double sz = ma.getElementAt(2, 2);
8881 final double mxy = ma.getElementAt(0, 1);
8882 final double mxz = ma.getElementAt(0, 2);
8883 final double myx = ma.getElementAt(1, 0);
8884 final double myz = ma.getElementAt(1, 2);
8885 final double mzx = ma.getElementAt(2, 0);
8886 final double mzy = ma.getElementAt(2, 1);
8887
8888 KnownPositionAccelerometerCalibrator calibrator =
8889 new KnownPositionAccelerometerCalibrator(measurements,
8890 true, ba, ma);
8891
8892
8893 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
8894 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
8895 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
8896 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
8897 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
8898 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8899 final Acceleration bx2 = new Acceleration(0.0,
8900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8901 calibrator.getInitialBiasXAsAcceleration(bx2);
8902 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
8903 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8904 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
8905 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
8906 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8907 final Acceleration by2 = new Acceleration(0.0,
8908 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8909 calibrator.getInitialBiasYAsAcceleration(by2);
8910 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
8911 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8912 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
8913 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
8914 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8915 final Acceleration bz2 = new Acceleration(0.0,
8916 AccelerationUnit.FEET_PER_SQUARED_SECOND);
8917 calibrator.getInitialBiasZAsAcceleration(bz2);
8918 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
8919 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
8920 assertEquals(calibrator.getInitialSx(), sx, 0.0);
8921 assertEquals(calibrator.getInitialSy(), sy, 0.0);
8922 assertEquals(calibrator.getInitialSz(), sz, 0.0);
8923 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
8924 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
8925 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
8926 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
8927 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
8928 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
8929 final double[] bias1 = calibrator.getInitialBias();
8930 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
8931 final double[] bias2 = new double[3];
8932 calibrator.getInitialBias(bias2);
8933 assertArrayEquals(bias1, bias2, 0.0);
8934 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
8935 assertEquals(b1, ba);
8936 final Matrix b2 = new Matrix(3, 1);
8937 calibrator.getInitialBiasAsMatrix(b2);
8938 assertEquals(b1, b2);
8939 final Matrix ma1 = new Matrix(3, 3);
8940 ma1.setSubmatrix(0, 0,
8941 2, 2,
8942 new double[]{sx, myx, mzx,
8943 mxy, sy, mzy,
8944 mxz, myz, sz});
8945 assertEquals(calibrator.getInitialMa(), ma1);
8946 final Matrix ma2 = new Matrix(3, 3);
8947 calibrator.getInitialMa(ma2);
8948 assertEquals(ma1, ma2);
8949 assertNull(calibrator.getEcefPosition());
8950 assertNull(calibrator.getNedPosition());
8951 assertFalse(calibrator.getNedPosition(null));
8952 assertSame(calibrator.getMeasurements(), measurements);
8953 assertTrue(calibrator.isCommonAxisUsed());
8954 assertNull(calibrator.getListener());
8955 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
8956 assertFalse(calibrator.isReady());
8957 assertFalse(calibrator.isRunning());
8958 assertNull(calibrator.getEstimatedBiases());
8959 assertFalse(calibrator.getEstimatedBiases(null));
8960 assertNull(calibrator.getEstimatedBiasesAsMatrix());
8961 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
8962 assertNull(calibrator.getEstimatedBiasFx());
8963 assertNull(calibrator.getEstimatedBiasFy());
8964 assertNull(calibrator.getEstimatedBiasFz());
8965 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
8966 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
8967 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
8968 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
8969 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
8970 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
8971 assertNull(calibrator.getEstimatedMa());
8972 assertNull(calibrator.getEstimatedSx());
8973 assertNull(calibrator.getEstimatedSy());
8974 assertNull(calibrator.getEstimatedSz());
8975 assertNull(calibrator.getEstimatedMxy());
8976 assertNull(calibrator.getEstimatedMxz());
8977 assertNull(calibrator.getEstimatedMyx());
8978 assertNull(calibrator.getEstimatedMyz());
8979 assertNull(calibrator.getEstimatedMzx());
8980 assertNull(calibrator.getEstimatedMzy());
8981 assertNull(calibrator.getEstimatedCovariance());
8982 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
8983 assertNull(calibrator.getGroundTruthGravityNorm());
8984 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
8985 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
8986
8987
8988 calibrator = null;
8989 try {
8990 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8991 true, new Matrix(1, 1), ma);
8992 fail("IllegalArgumentException expected but not thrown");
8993 } catch (final IllegalArgumentException ignore) {
8994 }
8995 try {
8996 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
8997 true, new Matrix(1, 3), ma);
8998 fail("IllegalArgumentException expected but not thrown");
8999 } catch (final IllegalArgumentException ignore) {
9000 }
9001 try {
9002 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9003 true, ba, new Matrix(1, 3));
9004 fail("IllegalArgumentException expected but not thrown");
9005 } catch (final IllegalArgumentException ignore) {
9006 }
9007 try {
9008 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9009 true, ba, new Matrix(3, 1));
9010 fail("IllegalArgumentException expected but not thrown");
9011 } catch (final IllegalArgumentException ignore) {
9012 }
9013 assertNull(calibrator);
9014 }
9015
9016 @Test
9017 public void testConstructor78() throws WrongSizeException {
9018 final Collection<StandardDeviationBodyKinematics> measurements =
9019 Collections.emptyList();
9020
9021 final Matrix ba = generateBa();
9022 final double biasX = ba.getElementAtIndex(0);
9023 final double biasY = ba.getElementAtIndex(1);
9024 final double biasZ = ba.getElementAtIndex(2);
9025
9026 final Matrix ma = generateMaCommonAxis();
9027 final double sx = ma.getElementAt(0, 0);
9028 final double sy = ma.getElementAt(1, 1);
9029 final double sz = ma.getElementAt(2, 2);
9030 final double mxy = ma.getElementAt(0, 1);
9031 final double mxz = ma.getElementAt(0, 2);
9032 final double myx = ma.getElementAt(1, 0);
9033 final double myz = ma.getElementAt(1, 2);
9034 final double mzx = ma.getElementAt(2, 0);
9035 final double mzy = ma.getElementAt(2, 1);
9036
9037 KnownPositionAccelerometerCalibrator calibrator =
9038 new KnownPositionAccelerometerCalibrator(measurements,
9039 true, ba, ma, this);
9040
9041
9042 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
9043 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
9044 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
9045 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9046 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
9047 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9048 final Acceleration bx2 = new Acceleration(0.0,
9049 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9050 calibrator.getInitialBiasXAsAcceleration(bx2);
9051 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
9052 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9053 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9054 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
9055 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9056 final Acceleration by2 = new Acceleration(0.0,
9057 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9058 calibrator.getInitialBiasYAsAcceleration(by2);
9059 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
9060 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9061 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9062 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
9063 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9064 final Acceleration bz2 = new Acceleration(0.0,
9065 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9066 calibrator.getInitialBiasZAsAcceleration(bz2);
9067 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
9068 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9069 assertEquals(calibrator.getInitialSx(), sx, 0.0);
9070 assertEquals(calibrator.getInitialSy(), sy, 0.0);
9071 assertEquals(calibrator.getInitialSz(), sz, 0.0);
9072 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
9073 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
9074 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
9075 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
9076 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
9077 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
9078 final double[] bias1 = calibrator.getInitialBias();
9079 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
9080 final double[] bias2 = new double[3];
9081 calibrator.getInitialBias(bias2);
9082 assertArrayEquals(bias1, bias2, 0.0);
9083 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9084 assertEquals(b1, ba);
9085 final Matrix b2 = new Matrix(3, 1);
9086 calibrator.getInitialBiasAsMatrix(b2);
9087 assertEquals(b1, b2);
9088 final Matrix ma1 = new Matrix(3, 3);
9089 ma1.setSubmatrix(0, 0,
9090 2, 2,
9091 new double[]{sx, myx, mzx,
9092 mxy, sy, mzy,
9093 mxz, myz, sz});
9094 assertEquals(calibrator.getInitialMa(), ma1);
9095 final Matrix ma2 = new Matrix(3, 3);
9096 calibrator.getInitialMa(ma2);
9097 assertEquals(ma1, ma2);
9098 assertNull(calibrator.getEcefPosition());
9099 assertNull(calibrator.getNedPosition());
9100 assertFalse(calibrator.getNedPosition(null));
9101 assertSame(calibrator.getMeasurements(), measurements);
9102 assertTrue(calibrator.isCommonAxisUsed());
9103 assertSame(calibrator.getListener(), this);
9104 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9105 assertFalse(calibrator.isReady());
9106 assertFalse(calibrator.isRunning());
9107 assertNull(calibrator.getEstimatedBiases());
9108 assertFalse(calibrator.getEstimatedBiases(null));
9109 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9110 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9111 assertNull(calibrator.getEstimatedBiasFx());
9112 assertNull(calibrator.getEstimatedBiasFy());
9113 assertNull(calibrator.getEstimatedBiasFz());
9114 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9115 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9116 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9117 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9118 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9119 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9120 assertNull(calibrator.getEstimatedMa());
9121 assertNull(calibrator.getEstimatedSx());
9122 assertNull(calibrator.getEstimatedSy());
9123 assertNull(calibrator.getEstimatedSz());
9124 assertNull(calibrator.getEstimatedMxy());
9125 assertNull(calibrator.getEstimatedMxz());
9126 assertNull(calibrator.getEstimatedMyx());
9127 assertNull(calibrator.getEstimatedMyz());
9128 assertNull(calibrator.getEstimatedMzx());
9129 assertNull(calibrator.getEstimatedMzy());
9130 assertNull(calibrator.getEstimatedCovariance());
9131 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9132 assertNull(calibrator.getGroundTruthGravityNorm());
9133 assertNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9134 assertFalse(calibrator.getGroundTruthGravityNormAsAcceleration(null));
9135
9136
9137 calibrator = null;
9138 try {
9139 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9140 true, new Matrix(1, 1), ma,
9141 this);
9142 fail("IllegalArgumentException expected but not thrown");
9143 } catch (final IllegalArgumentException ignore) {
9144 }
9145 try {
9146 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9147 true, new Matrix(1, 3), ma,
9148 this);
9149 fail("IllegalArgumentException expected but not thrown");
9150 } catch (final IllegalArgumentException ignore) {
9151 }
9152 try {
9153 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9154 true, ba, new Matrix(1, 3),
9155 this);
9156 fail("IllegalArgumentException expected but not thrown");
9157 } catch (final IllegalArgumentException ignore) {
9158 }
9159 try {
9160 calibrator = new KnownPositionAccelerometerCalibrator(measurements,
9161 true, ba, new Matrix(3, 1),
9162 this);
9163 fail("IllegalArgumentException expected but not thrown");
9164 } catch (final IllegalArgumentException ignore) {
9165 }
9166 assertNull(calibrator);
9167 }
9168
9169 @Test
9170 public void testConstructor79() throws WrongSizeException {
9171 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9172 final double latitude = Math.toRadians(
9173 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9174 final double longitude = Math.toRadians(
9175 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9176 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9177 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9178 final NEDVelocity nedVelocity = new NEDVelocity();
9179 final ECEFPosition ecefPosition = new ECEFPosition();
9180 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9181 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9182 ecefPosition, ecefVelocity);
9183
9184 final KnownPositionAccelerometerCalibrator calibrator =
9185 new KnownPositionAccelerometerCalibrator(ecefPosition);
9186
9187
9188 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9189 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9190 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9191 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9192 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9193 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9194 final Acceleration bx2 = new Acceleration(0.0,
9195 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9196 calibrator.getInitialBiasXAsAcceleration(bx2);
9197 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9198 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9199 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9200 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9201 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9202 final Acceleration by2 = new Acceleration(0.0,
9203 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9204 calibrator.getInitialBiasYAsAcceleration(by2);
9205 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9206 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9207 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9208 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9209 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9210 final Acceleration bz2 = new Acceleration(0.0,
9211 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9212 calibrator.getInitialBiasZAsAcceleration(bz2);
9213 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9214 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9215 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9216 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9217 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9218 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9219 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9220 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9221 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9222 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9223 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9224 final double[] bias1 = calibrator.getInitialBias();
9225 assertArrayEquals(bias1, new double[3], 0.0);
9226 final double[] bias2 = new double[3];
9227 calibrator.getInitialBias(bias2);
9228 assertArrayEquals(bias1, bias2, 0.0);
9229 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9230 assertEquals(b1, new Matrix(3, 1));
9231 final Matrix b2 = new Matrix(3, 1);
9232 calibrator.getInitialBiasAsMatrix(b2);
9233 assertEquals(b1, b2);
9234 final Matrix ma1 = calibrator.getInitialMa();
9235 assertEquals(ma1, new Matrix(3, 3));
9236 final Matrix ma2 = new Matrix(3, 3);
9237 calibrator.getInitialMa(ma2);
9238 assertEquals(ma1, ma2);
9239 assertSame(calibrator.getEcefPosition(), ecefPosition);
9240 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9241 final NEDPosition nedPosition1 = new NEDPosition();
9242 assertTrue(calibrator.getNedPosition(nedPosition1));
9243 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9244 assertNull(calibrator.getMeasurements());
9245 assertFalse(calibrator.isCommonAxisUsed());
9246 assertNull(calibrator.getListener());
9247 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9248 assertFalse(calibrator.isReady());
9249 assertFalse(calibrator.isRunning());
9250 assertNull(calibrator.getEstimatedBiases());
9251 assertFalse(calibrator.getEstimatedBiases(null));
9252 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9253 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9254 assertNull(calibrator.getEstimatedBiasFx());
9255 assertNull(calibrator.getEstimatedBiasFy());
9256 assertNull(calibrator.getEstimatedBiasFz());
9257 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9258 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9259 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9260 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9261 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9262 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9263 assertNull(calibrator.getEstimatedMa());
9264 assertNull(calibrator.getEstimatedSx());
9265 assertNull(calibrator.getEstimatedSy());
9266 assertNull(calibrator.getEstimatedSz());
9267 assertNull(calibrator.getEstimatedMxy());
9268 assertNull(calibrator.getEstimatedMxz());
9269 assertNull(calibrator.getEstimatedMyx());
9270 assertNull(calibrator.getEstimatedMyz());
9271 assertNull(calibrator.getEstimatedMzx());
9272 assertNull(calibrator.getEstimatedMzy());
9273 assertNull(calibrator.getEstimatedCovariance());
9274 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9275 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9276 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9277 assertNotNull(calibrator.getGroundTruthGravityNorm());
9278 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9279 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9280 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9281 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9282 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9283 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9284 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9285 }
9286
9287 @Test
9288 public void testConstructor80() throws WrongSizeException {
9289 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9290 final double latitude = Math.toRadians(
9291 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9292 final double longitude = Math.toRadians(
9293 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9294 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9295 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9296 final NEDVelocity nedVelocity = new NEDVelocity();
9297 final ECEFPosition ecefPosition = new ECEFPosition();
9298 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9299 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9300 ecefPosition, ecefVelocity);
9301
9302 final KnownPositionAccelerometerCalibrator calibrator =
9303 new KnownPositionAccelerometerCalibrator(ecefPosition, this);
9304
9305
9306 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9307 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9308 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9309 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9310 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9311 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9312 final Acceleration bx2 = new Acceleration(0.0,
9313 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9314 calibrator.getInitialBiasXAsAcceleration(bx2);
9315 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9316 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9317 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9318 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9319 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9320 final Acceleration by2 = new Acceleration(0.0,
9321 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9322 calibrator.getInitialBiasYAsAcceleration(by2);
9323 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9324 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9325 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9326 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9327 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9328 final Acceleration bz2 = new Acceleration(0.0,
9329 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9330 calibrator.getInitialBiasZAsAcceleration(bz2);
9331 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9332 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9333 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9334 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9335 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9336 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9337 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9338 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9339 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9340 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9341 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9342 final double[] bias1 = calibrator.getInitialBias();
9343 assertArrayEquals(bias1, new double[3], 0.0);
9344 final double[] bias2 = new double[3];
9345 calibrator.getInitialBias(bias2);
9346 assertArrayEquals(bias1, bias2, 0.0);
9347 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9348 assertEquals(b1, new Matrix(3, 1));
9349 final Matrix b2 = new Matrix(3, 1);
9350 calibrator.getInitialBiasAsMatrix(b2);
9351 assertEquals(b1, b2);
9352 final Matrix ma1 = calibrator.getInitialMa();
9353 assertEquals(ma1, new Matrix(3, 3));
9354 final Matrix ma2 = new Matrix(3, 3);
9355 calibrator.getInitialMa(ma2);
9356 assertEquals(ma1, ma2);
9357 assertSame(calibrator.getEcefPosition(), ecefPosition);
9358 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9359 final NEDPosition nedPosition1 = new NEDPosition();
9360 assertTrue(calibrator.getNedPosition(nedPosition1));
9361 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9362 assertNull(calibrator.getMeasurements());
9363 assertFalse(calibrator.isCommonAxisUsed());
9364 assertSame(calibrator.getListener(), this);
9365 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9366 assertFalse(calibrator.isReady());
9367 assertFalse(calibrator.isRunning());
9368 assertNull(calibrator.getEstimatedBiases());
9369 assertFalse(calibrator.getEstimatedBiases(null));
9370 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9371 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9372 assertNull(calibrator.getEstimatedBiasFx());
9373 assertNull(calibrator.getEstimatedBiasFy());
9374 assertNull(calibrator.getEstimatedBiasFz());
9375 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9376 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9377 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9378 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9379 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9380 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9381 assertNull(calibrator.getEstimatedMa());
9382 assertNull(calibrator.getEstimatedSx());
9383 assertNull(calibrator.getEstimatedSy());
9384 assertNull(calibrator.getEstimatedSz());
9385 assertNull(calibrator.getEstimatedMxy());
9386 assertNull(calibrator.getEstimatedMxz());
9387 assertNull(calibrator.getEstimatedMyx());
9388 assertNull(calibrator.getEstimatedMyz());
9389 assertNull(calibrator.getEstimatedMzx());
9390 assertNull(calibrator.getEstimatedMzy());
9391 assertNull(calibrator.getEstimatedCovariance());
9392 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9393 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9394 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9395 assertNotNull(calibrator.getGroundTruthGravityNorm());
9396 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9397 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9398 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9399 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9400 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9401 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9402 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9403 }
9404
9405 @Test
9406 public void testConstructor81() throws WrongSizeException {
9407 final Collection<StandardDeviationBodyKinematics> measurements =
9408 Collections.emptyList();
9409
9410 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9411 final double latitude = Math.toRadians(
9412 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9413 final double longitude = Math.toRadians(
9414 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9415 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9416 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9417 final NEDVelocity nedVelocity = new NEDVelocity();
9418 final ECEFPosition ecefPosition = new ECEFPosition();
9419 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9420 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9421 ecefPosition, ecefVelocity);
9422
9423 final KnownPositionAccelerometerCalibrator calibrator =
9424 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements);
9425
9426
9427 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9428 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9429 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9430 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9431 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9432 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9433 final Acceleration bx2 = new Acceleration(0.0,
9434 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9435 calibrator.getInitialBiasXAsAcceleration(bx2);
9436 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9437 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9438 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9439 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9440 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9441 final Acceleration by2 = new Acceleration(0.0,
9442 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9443 calibrator.getInitialBiasYAsAcceleration(by2);
9444 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9445 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9446 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9447 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9448 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9449 final Acceleration bz2 = new Acceleration(0.0,
9450 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9451 calibrator.getInitialBiasZAsAcceleration(bz2);
9452 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9453 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9454 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9455 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9456 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9457 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9458 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9459 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9460 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9461 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9462 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9463 final double[] bias1 = calibrator.getInitialBias();
9464 assertArrayEquals(bias1, new double[3], 0.0);
9465 final double[] bias2 = new double[3];
9466 calibrator.getInitialBias(bias2);
9467 assertArrayEquals(bias1, bias2, 0.0);
9468 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9469 assertEquals(b1, new Matrix(3, 1));
9470 final Matrix b2 = new Matrix(3, 1);
9471 calibrator.getInitialBiasAsMatrix(b2);
9472 assertEquals(b1, b2);
9473 final Matrix ma1 = calibrator.getInitialMa();
9474 assertEquals(ma1, new Matrix(3, 3));
9475 final Matrix ma2 = new Matrix(3, 3);
9476 calibrator.getInitialMa(ma2);
9477 assertEquals(ma1, ma2);
9478 assertSame(calibrator.getEcefPosition(), ecefPosition);
9479 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9480 final NEDPosition nedPosition1 = new NEDPosition();
9481 assertTrue(calibrator.getNedPosition(nedPosition1));
9482 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9483 assertSame(calibrator.getMeasurements(), measurements);
9484 assertFalse(calibrator.isCommonAxisUsed());
9485 assertNull(calibrator.getListener());
9486 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9487 assertFalse(calibrator.isReady());
9488 assertFalse(calibrator.isRunning());
9489 assertNull(calibrator.getEstimatedBiases());
9490 assertFalse(calibrator.getEstimatedBiases(null));
9491 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9492 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9493 assertNull(calibrator.getEstimatedBiasFx());
9494 assertNull(calibrator.getEstimatedBiasFy());
9495 assertNull(calibrator.getEstimatedBiasFz());
9496 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9497 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9498 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9499 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9500 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9501 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9502 assertNull(calibrator.getEstimatedMa());
9503 assertNull(calibrator.getEstimatedSx());
9504 assertNull(calibrator.getEstimatedSy());
9505 assertNull(calibrator.getEstimatedSz());
9506 assertNull(calibrator.getEstimatedMxy());
9507 assertNull(calibrator.getEstimatedMxz());
9508 assertNull(calibrator.getEstimatedMyx());
9509 assertNull(calibrator.getEstimatedMyz());
9510 assertNull(calibrator.getEstimatedMzx());
9511 assertNull(calibrator.getEstimatedMzy());
9512 assertNull(calibrator.getEstimatedCovariance());
9513 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9514 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9515 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9516 assertNotNull(calibrator.getGroundTruthGravityNorm());
9517 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9518 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9519 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9520 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9521 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9522 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9523 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9524 }
9525
9526 @Test
9527 public void testConstructor82() throws WrongSizeException {
9528 final Collection<StandardDeviationBodyKinematics> measurements =
9529 Collections.emptyList();
9530
9531 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9532 final double latitude = Math.toRadians(
9533 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9534 final double longitude = Math.toRadians(
9535 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9536 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9537 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9538 final NEDVelocity nedVelocity = new NEDVelocity();
9539 final ECEFPosition ecefPosition = new ECEFPosition();
9540 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9541 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9542 ecefPosition, ecefVelocity);
9543
9544 final KnownPositionAccelerometerCalibrator calibrator =
9545 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9546 this);
9547
9548
9549 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9550 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9551 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9552 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9553 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9554 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9555 final Acceleration bx2 = new Acceleration(0.0,
9556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9557 calibrator.getInitialBiasXAsAcceleration(bx2);
9558 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9559 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9560 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9561 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9562 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9563 final Acceleration by2 = new Acceleration(0.0,
9564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9565 calibrator.getInitialBiasYAsAcceleration(by2);
9566 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9567 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9568 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9569 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9570 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9571 final Acceleration bz2 = new Acceleration(0.0,
9572 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9573 calibrator.getInitialBiasZAsAcceleration(bz2);
9574 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9575 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9576 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9577 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9578 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9579 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9580 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9581 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9582 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9583 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9584 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9585 final double[] bias1 = calibrator.getInitialBias();
9586 assertArrayEquals(bias1, new double[3], 0.0);
9587 final double[] bias2 = new double[3];
9588 calibrator.getInitialBias(bias2);
9589 assertArrayEquals(bias1, bias2, 0.0);
9590 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9591 assertEquals(b1, new Matrix(3, 1));
9592 final Matrix b2 = new Matrix(3, 1);
9593 calibrator.getInitialBiasAsMatrix(b2);
9594 assertEquals(b1, b2);
9595 final Matrix ma1 = calibrator.getInitialMa();
9596 assertEquals(ma1, new Matrix(3, 3));
9597 final Matrix ma2 = new Matrix(3, 3);
9598 calibrator.getInitialMa(ma2);
9599 assertEquals(ma1, ma2);
9600 assertSame(calibrator.getEcefPosition(), ecefPosition);
9601 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9602 final NEDPosition nedPosition1 = new NEDPosition();
9603 assertTrue(calibrator.getNedPosition(nedPosition1));
9604 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9605 assertSame(calibrator.getMeasurements(), measurements);
9606 assertFalse(calibrator.isCommonAxisUsed());
9607 assertSame(calibrator.getListener(), this);
9608 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
9609 assertFalse(calibrator.isReady());
9610 assertFalse(calibrator.isRunning());
9611 assertNull(calibrator.getEstimatedBiases());
9612 assertFalse(calibrator.getEstimatedBiases(null));
9613 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9614 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9615 assertNull(calibrator.getEstimatedBiasFx());
9616 assertNull(calibrator.getEstimatedBiasFy());
9617 assertNull(calibrator.getEstimatedBiasFz());
9618 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9619 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9620 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9621 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9622 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9623 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9624 assertNull(calibrator.getEstimatedMa());
9625 assertNull(calibrator.getEstimatedSx());
9626 assertNull(calibrator.getEstimatedSy());
9627 assertNull(calibrator.getEstimatedSz());
9628 assertNull(calibrator.getEstimatedMxy());
9629 assertNull(calibrator.getEstimatedMxz());
9630 assertNull(calibrator.getEstimatedMyx());
9631 assertNull(calibrator.getEstimatedMyz());
9632 assertNull(calibrator.getEstimatedMzx());
9633 assertNull(calibrator.getEstimatedMzy());
9634 assertNull(calibrator.getEstimatedCovariance());
9635 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9636 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9637 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9638 assertNotNull(calibrator.getGroundTruthGravityNorm());
9639 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9640 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9641 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9642 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9643 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9644 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9645 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9646 }
9647
9648 @Test
9649 public void testConstructor83() throws WrongSizeException {
9650 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9651 final double latitude = Math.toRadians(
9652 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9653 final double longitude = Math.toRadians(
9654 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9655 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9656 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9657 final NEDVelocity nedVelocity = new NEDVelocity();
9658 final ECEFPosition ecefPosition = new ECEFPosition();
9659 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9660 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9661 ecefPosition, ecefVelocity);
9662
9663 final KnownPositionAccelerometerCalibrator calibrator =
9664 new KnownPositionAccelerometerCalibrator(ecefPosition,
9665 true);
9666
9667
9668 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9669 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9670 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9671 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9672 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9673 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9674 final Acceleration bx2 = new Acceleration(0.0,
9675 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9676 calibrator.getInitialBiasXAsAcceleration(bx2);
9677 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9678 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9679 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9680 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9681 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9682 final Acceleration by2 = new Acceleration(0.0,
9683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9684 calibrator.getInitialBiasYAsAcceleration(by2);
9685 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9686 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9687 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9688 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9689 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9690 final Acceleration bz2 = new Acceleration(0.0,
9691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9692 calibrator.getInitialBiasZAsAcceleration(bz2);
9693 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9694 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9695 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9696 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9697 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9698 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9699 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9700 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9701 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9702 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9703 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9704 final double[] bias1 = calibrator.getInitialBias();
9705 assertArrayEquals(bias1, new double[3], 0.0);
9706 final double[] bias2 = new double[3];
9707 calibrator.getInitialBias(bias2);
9708 assertArrayEquals(bias1, bias2, 0.0);
9709 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9710 assertEquals(b1, new Matrix(3, 1));
9711 final Matrix b2 = new Matrix(3, 1);
9712 calibrator.getInitialBiasAsMatrix(b2);
9713 assertEquals(b1, b2);
9714 final Matrix ma1 = calibrator.getInitialMa();
9715 assertEquals(ma1, new Matrix(3, 3));
9716 final Matrix ma2 = new Matrix(3, 3);
9717 calibrator.getInitialMa(ma2);
9718 assertEquals(ma1, ma2);
9719 assertSame(calibrator.getEcefPosition(), ecefPosition);
9720 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9721 final NEDPosition nedPosition1 = new NEDPosition();
9722 assertTrue(calibrator.getNedPosition(nedPosition1));
9723 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9724 assertNull(calibrator.getMeasurements());
9725 assertTrue(calibrator.isCommonAxisUsed());
9726 assertNull(calibrator.getListener());
9727 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9728 assertFalse(calibrator.isReady());
9729 assertFalse(calibrator.isRunning());
9730 assertNull(calibrator.getEstimatedBiases());
9731 assertFalse(calibrator.getEstimatedBiases(null));
9732 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9733 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9734 assertNull(calibrator.getEstimatedBiasFx());
9735 assertNull(calibrator.getEstimatedBiasFy());
9736 assertNull(calibrator.getEstimatedBiasFz());
9737 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9738 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9739 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9740 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9741 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9742 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9743 assertNull(calibrator.getEstimatedMa());
9744 assertNull(calibrator.getEstimatedSx());
9745 assertNull(calibrator.getEstimatedSy());
9746 assertNull(calibrator.getEstimatedSz());
9747 assertNull(calibrator.getEstimatedMxy());
9748 assertNull(calibrator.getEstimatedMxz());
9749 assertNull(calibrator.getEstimatedMyx());
9750 assertNull(calibrator.getEstimatedMyz());
9751 assertNull(calibrator.getEstimatedMzx());
9752 assertNull(calibrator.getEstimatedMzy());
9753 assertNull(calibrator.getEstimatedCovariance());
9754 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9755 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9756 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9757 assertNotNull(calibrator.getGroundTruthGravityNorm());
9758 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9759 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9760 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9761 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9762 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9763 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9764 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9765 }
9766
9767 @Test
9768 public void testConstructor84() throws WrongSizeException {
9769 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9770 final double latitude = Math.toRadians(
9771 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9772 final double longitude = Math.toRadians(
9773 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9774 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9775 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9776 final NEDVelocity nedVelocity = new NEDVelocity();
9777 final ECEFPosition ecefPosition = new ECEFPosition();
9778 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9779 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9780 ecefPosition, ecefVelocity);
9781
9782 final KnownPositionAccelerometerCalibrator calibrator =
9783 new KnownPositionAccelerometerCalibrator(ecefPosition,
9784 true, this);
9785
9786
9787 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9788 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9789 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9790 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9791 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9792 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9793 final Acceleration bx2 = new Acceleration(0.0,
9794 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9795 calibrator.getInitialBiasXAsAcceleration(bx2);
9796 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9797 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9798 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9799 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9800 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9801 final Acceleration by2 = new Acceleration(0.0,
9802 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9803 calibrator.getInitialBiasYAsAcceleration(by2);
9804 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9805 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9806 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9807 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9808 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9809 final Acceleration bz2 = new Acceleration(0.0,
9810 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9811 calibrator.getInitialBiasZAsAcceleration(bz2);
9812 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9813 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9814 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9815 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9816 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9817 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9818 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9819 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9820 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9821 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9822 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9823 final double[] bias1 = calibrator.getInitialBias();
9824 assertArrayEquals(bias1, new double[3], 0.0);
9825 final double[] bias2 = new double[3];
9826 calibrator.getInitialBias(bias2);
9827 assertArrayEquals(bias1, bias2, 0.0);
9828 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9829 assertEquals(b1, new Matrix(3, 1));
9830 final Matrix b2 = new Matrix(3, 1);
9831 calibrator.getInitialBiasAsMatrix(b2);
9832 assertEquals(b1, b2);
9833 final Matrix ma1 = calibrator.getInitialMa();
9834 assertEquals(ma1, new Matrix(3, 3));
9835 final Matrix ma2 = new Matrix(3, 3);
9836 calibrator.getInitialMa(ma2);
9837 assertEquals(ma1, ma2);
9838 assertSame(calibrator.getEcefPosition(), ecefPosition);
9839 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9840 final NEDPosition nedPosition1 = new NEDPosition();
9841 assertTrue(calibrator.getNedPosition(nedPosition1));
9842 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9843 assertNull(calibrator.getMeasurements());
9844 assertTrue(calibrator.isCommonAxisUsed());
9845 assertSame(calibrator.getListener(), this);
9846 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9847 assertFalse(calibrator.isReady());
9848 assertFalse(calibrator.isRunning());
9849 assertNull(calibrator.getEstimatedBiases());
9850 assertFalse(calibrator.getEstimatedBiases(null));
9851 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9852 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9853 assertNull(calibrator.getEstimatedBiasFx());
9854 assertNull(calibrator.getEstimatedBiasFy());
9855 assertNull(calibrator.getEstimatedBiasFz());
9856 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9857 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9858 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9859 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9860 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9861 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9862 assertNull(calibrator.getEstimatedMa());
9863 assertNull(calibrator.getEstimatedSx());
9864 assertNull(calibrator.getEstimatedSy());
9865 assertNull(calibrator.getEstimatedSz());
9866 assertNull(calibrator.getEstimatedMxy());
9867 assertNull(calibrator.getEstimatedMxz());
9868 assertNull(calibrator.getEstimatedMyx());
9869 assertNull(calibrator.getEstimatedMyz());
9870 assertNull(calibrator.getEstimatedMzx());
9871 assertNull(calibrator.getEstimatedMzy());
9872 assertNull(calibrator.getEstimatedCovariance());
9873 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9874 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9875 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9876 assertNotNull(calibrator.getGroundTruthGravityNorm());
9877 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9878 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
9879 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
9880 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
9881 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
9882 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
9883 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
9884 }
9885
9886 @Test
9887 public void testConstructor85() throws WrongSizeException {
9888 final Collection<StandardDeviationBodyKinematics> measurements =
9889 Collections.emptyList();
9890 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
9891 final double latitude = Math.toRadians(
9892 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
9893 final double longitude = Math.toRadians(
9894 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
9895 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
9896 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
9897 final NEDVelocity nedVelocity = new NEDVelocity();
9898 final ECEFPosition ecefPosition = new ECEFPosition();
9899 final ECEFVelocity ecefVelocity = new ECEFVelocity();
9900 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
9901 ecefPosition, ecefVelocity);
9902
9903 final KnownPositionAccelerometerCalibrator calibrator =
9904 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
9905 true);
9906
9907
9908 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
9909 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
9910 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
9911 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
9912 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
9913 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9914 final Acceleration bx2 = new Acceleration(0.0,
9915 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9916 calibrator.getInitialBiasXAsAcceleration(bx2);
9917 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
9918 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9919 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
9920 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
9921 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9922 final Acceleration by2 = new Acceleration(0.0,
9923 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9924 calibrator.getInitialBiasYAsAcceleration(by2);
9925 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
9926 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9927 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
9928 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
9929 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9930 final Acceleration bz2 = new Acceleration(0.0,
9931 AccelerationUnit.FEET_PER_SQUARED_SECOND);
9932 calibrator.getInitialBiasZAsAcceleration(bz2);
9933 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
9934 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
9935 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
9936 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
9937 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
9938 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
9939 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
9940 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
9941 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
9942 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
9943 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
9944 final double[] bias1 = calibrator.getInitialBias();
9945 assertArrayEquals(bias1, new double[3], 0.0);
9946 final double[] bias2 = new double[3];
9947 calibrator.getInitialBias(bias2);
9948 assertArrayEquals(bias1, bias2, 0.0);
9949 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
9950 assertEquals(b1, new Matrix(3, 1));
9951 final Matrix b2 = new Matrix(3, 1);
9952 calibrator.getInitialBiasAsMatrix(b2);
9953 assertEquals(b1, b2);
9954 final Matrix ma1 = calibrator.getInitialMa();
9955 assertEquals(ma1, new Matrix(3, 3));
9956 final Matrix ma2 = new Matrix(3, 3);
9957 calibrator.getInitialMa(ma2);
9958 assertEquals(ma1, ma2);
9959 assertSame(calibrator.getEcefPosition(), ecefPosition);
9960 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
9961 final NEDPosition nedPosition1 = new NEDPosition();
9962 assertTrue(calibrator.getNedPosition(nedPosition1));
9963 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
9964 assertSame(calibrator.getMeasurements(), measurements);
9965 assertTrue(calibrator.isCommonAxisUsed());
9966 assertNull(calibrator.getListener());
9967 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
9968 assertFalse(calibrator.isReady());
9969 assertFalse(calibrator.isRunning());
9970 assertNull(calibrator.getEstimatedBiases());
9971 assertFalse(calibrator.getEstimatedBiases(null));
9972 assertNull(calibrator.getEstimatedBiasesAsMatrix());
9973 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
9974 assertNull(calibrator.getEstimatedBiasFx());
9975 assertNull(calibrator.getEstimatedBiasFy());
9976 assertNull(calibrator.getEstimatedBiasFz());
9977 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
9978 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
9979 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
9980 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
9981 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
9982 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
9983 assertNull(calibrator.getEstimatedMa());
9984 assertNull(calibrator.getEstimatedSx());
9985 assertNull(calibrator.getEstimatedSy());
9986 assertNull(calibrator.getEstimatedSz());
9987 assertNull(calibrator.getEstimatedMxy());
9988 assertNull(calibrator.getEstimatedMxz());
9989 assertNull(calibrator.getEstimatedMyx());
9990 assertNull(calibrator.getEstimatedMyz());
9991 assertNull(calibrator.getEstimatedMzx());
9992 assertNull(calibrator.getEstimatedMzy());
9993 assertNull(calibrator.getEstimatedCovariance());
9994 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
9995 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
9996 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
9997 assertNotNull(calibrator.getGroundTruthGravityNorm());
9998 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
9999 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10000 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10001 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10002 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10003 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10004 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10005 }
10006
10007 @Test
10008 public void testConstructor86() throws WrongSizeException {
10009 final Collection<StandardDeviationBodyKinematics> measurements =
10010 Collections.emptyList();
10011 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10012 final double latitude = Math.toRadians(
10013 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10014 final double longitude = Math.toRadians(
10015 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10016 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10017 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10018 final NEDVelocity nedVelocity = new NEDVelocity();
10019 final ECEFPosition ecefPosition = new ECEFPosition();
10020 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10021 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10022 ecefPosition, ecefVelocity);
10023
10024 final KnownPositionAccelerometerCalibrator calibrator =
10025 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10026 true, this);
10027
10028
10029 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
10030 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
10031 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
10032 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10033 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
10034 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10035 final Acceleration bx2 = new Acceleration(0.0,
10036 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10037 calibrator.getInitialBiasXAsAcceleration(bx2);
10038 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
10039 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10040 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10041 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
10042 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10043 final Acceleration by2 = new Acceleration(0.0,
10044 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10045 calibrator.getInitialBiasYAsAcceleration(by2);
10046 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
10047 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10048 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10049 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
10050 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10051 final Acceleration bz2 = new Acceleration(0.0,
10052 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10053 calibrator.getInitialBiasZAsAcceleration(bz2);
10054 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
10055 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10056 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10057 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10058 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10059 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10060 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10061 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10062 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10063 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10064 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10065 final double[] bias1 = calibrator.getInitialBias();
10066 assertArrayEquals(bias1, new double[3], 0.0);
10067 final double[] bias2 = new double[3];
10068 calibrator.getInitialBias(bias2);
10069 assertArrayEquals(bias1, bias2, 0.0);
10070 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10071 assertEquals(b1, new Matrix(3, 1));
10072 final Matrix b2 = new Matrix(3, 1);
10073 calibrator.getInitialBiasAsMatrix(b2);
10074 assertEquals(b1, b2);
10075 final Matrix ma1 = calibrator.getInitialMa();
10076 assertEquals(ma1, new Matrix(3, 3));
10077 final Matrix ma2 = new Matrix(3, 3);
10078 calibrator.getInitialMa(ma2);
10079 assertEquals(ma1, ma2);
10080 assertSame(calibrator.getEcefPosition(), ecefPosition);
10081 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10082 final NEDPosition nedPosition1 = new NEDPosition();
10083 assertTrue(calibrator.getNedPosition(nedPosition1));
10084 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10085 assertSame(calibrator.getMeasurements(), measurements);
10086 assertTrue(calibrator.isCommonAxisUsed());
10087 assertSame(calibrator.getListener(), this);
10088 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10089 assertFalse(calibrator.isReady());
10090 assertFalse(calibrator.isRunning());
10091 assertNull(calibrator.getEstimatedBiases());
10092 assertFalse(calibrator.getEstimatedBiases(null));
10093 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10094 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10095 assertNull(calibrator.getEstimatedBiasFx());
10096 assertNull(calibrator.getEstimatedBiasFy());
10097 assertNull(calibrator.getEstimatedBiasFz());
10098 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10099 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10100 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10101 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10102 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10103 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10104 assertNull(calibrator.getEstimatedMa());
10105 assertNull(calibrator.getEstimatedSx());
10106 assertNull(calibrator.getEstimatedSy());
10107 assertNull(calibrator.getEstimatedSz());
10108 assertNull(calibrator.getEstimatedMxy());
10109 assertNull(calibrator.getEstimatedMxz());
10110 assertNull(calibrator.getEstimatedMyx());
10111 assertNull(calibrator.getEstimatedMyz());
10112 assertNull(calibrator.getEstimatedMzx());
10113 assertNull(calibrator.getEstimatedMzy());
10114 assertNull(calibrator.getEstimatedCovariance());
10115 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10116 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10117 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10118 assertNotNull(calibrator.getGroundTruthGravityNorm());
10119 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10120 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10121 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10122 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10123 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10124 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10125 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10126 }
10127
10128 @Test
10129 public void testConstructor87() throws WrongSizeException {
10130 final Matrix ba = generateBa();
10131 final double biasX = ba.getElementAtIndex(0);
10132 final double biasY = ba.getElementAtIndex(1);
10133 final double biasZ = ba.getElementAtIndex(2);
10134
10135 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10136 final double latitude = Math.toRadians(
10137 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10138 final double longitude = Math.toRadians(
10139 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10140 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10141 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10142 final NEDVelocity nedVelocity = new NEDVelocity();
10143 final ECEFPosition ecefPosition = new ECEFPosition();
10144 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10145 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10146 ecefPosition, ecefVelocity);
10147
10148 final KnownPositionAccelerometerCalibrator calibrator =
10149 new KnownPositionAccelerometerCalibrator(ecefPosition,
10150 biasX, biasY, biasZ);
10151
10152
10153 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10154 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10155 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10156 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10157 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10158 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10159 final Acceleration bx2 = new Acceleration(0.0,
10160 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10161 calibrator.getInitialBiasXAsAcceleration(bx2);
10162 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10163 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10164 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10165 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10166 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10167 final Acceleration by2 = new Acceleration(0.0,
10168 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10169 calibrator.getInitialBiasYAsAcceleration(by2);
10170 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10171 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10172 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10173 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10174 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10175 final Acceleration bz2 = new Acceleration(0.0,
10176 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10177 calibrator.getInitialBiasZAsAcceleration(bz2);
10178 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10179 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10180 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10181 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10182 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10183 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10184 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10185 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10186 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10187 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10188 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10189 final double[] bias1 = calibrator.getInitialBias();
10190 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10191 final double[] bias2 = new double[3];
10192 calibrator.getInitialBias(bias2);
10193 assertArrayEquals(bias1, bias2, 0.0);
10194 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10195 assertEquals(b1, ba);
10196 final Matrix b2 = new Matrix(3, 1);
10197 calibrator.getInitialBiasAsMatrix(b2);
10198 assertEquals(b1, b2);
10199 final Matrix ma1 = calibrator.getInitialMa();
10200 assertEquals(ma1, new Matrix(3, 3));
10201 final Matrix ma2 = new Matrix(3, 3);
10202 calibrator.getInitialMa(ma2);
10203 assertEquals(ma1, ma2);
10204 assertSame(calibrator.getEcefPosition(), ecefPosition);
10205 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10206 final NEDPosition nedPosition1 = new NEDPosition();
10207 assertTrue(calibrator.getNedPosition(nedPosition1));
10208 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10209 assertNull(calibrator.getMeasurements());
10210 assertFalse(calibrator.isCommonAxisUsed());
10211 assertNull(calibrator.getListener());
10212 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10213 assertFalse(calibrator.isReady());
10214 assertFalse(calibrator.isRunning());
10215 assertNull(calibrator.getEstimatedBiases());
10216 assertFalse(calibrator.getEstimatedBiases(null));
10217 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10218 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10219 assertNull(calibrator.getEstimatedBiasFx());
10220 assertNull(calibrator.getEstimatedBiasFy());
10221 assertNull(calibrator.getEstimatedBiasFz());
10222 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10223 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10224 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10225 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10226 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10227 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10228 assertNull(calibrator.getEstimatedMa());
10229 assertNull(calibrator.getEstimatedSx());
10230 assertNull(calibrator.getEstimatedSy());
10231 assertNull(calibrator.getEstimatedSz());
10232 assertNull(calibrator.getEstimatedMxy());
10233 assertNull(calibrator.getEstimatedMxz());
10234 assertNull(calibrator.getEstimatedMyx());
10235 assertNull(calibrator.getEstimatedMyz());
10236 assertNull(calibrator.getEstimatedMzx());
10237 assertNull(calibrator.getEstimatedMzy());
10238 assertNull(calibrator.getEstimatedCovariance());
10239 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10240 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10241 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10242 assertNotNull(calibrator.getGroundTruthGravityNorm());
10243 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10244 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10245 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10246 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10247 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10248 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10249 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10250 }
10251
10252 @Test
10253 public void testConstructor88() throws WrongSizeException {
10254 final Matrix ba = generateBa();
10255 final double biasX = ba.getElementAtIndex(0);
10256 final double biasY = ba.getElementAtIndex(1);
10257 final double biasZ = ba.getElementAtIndex(2);
10258
10259 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10260 final double latitude = Math.toRadians(
10261 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10262 final double longitude = Math.toRadians(
10263 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10264 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10265 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10266 final NEDVelocity nedVelocity = new NEDVelocity();
10267 final ECEFPosition ecefPosition = new ECEFPosition();
10268 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10269 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10270 ecefPosition, ecefVelocity);
10271
10272 final KnownPositionAccelerometerCalibrator calibrator =
10273 new KnownPositionAccelerometerCalibrator(ecefPosition,
10274 biasX, biasY, biasZ, this);
10275
10276
10277 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10278 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10279 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10280 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10281 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10282 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10283 final Acceleration bx2 = new Acceleration(0.0,
10284 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10285 calibrator.getInitialBiasXAsAcceleration(bx2);
10286 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10287 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10288 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10289 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10290 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10291 final Acceleration by2 = new Acceleration(0.0,
10292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10293 calibrator.getInitialBiasYAsAcceleration(by2);
10294 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10295 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10296 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10297 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10298 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10299 final Acceleration bz2 = new Acceleration(0.0,
10300 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10301 calibrator.getInitialBiasZAsAcceleration(bz2);
10302 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10303 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10304 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10305 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10306 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10307 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10308 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10309 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10310 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10311 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10312 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10313 final double[] bias1 = calibrator.getInitialBias();
10314 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10315 final double[] bias2 = new double[3];
10316 calibrator.getInitialBias(bias2);
10317 assertArrayEquals(bias1, bias2, 0.0);
10318 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10319 assertEquals(b1, ba);
10320 final Matrix b2 = new Matrix(3, 1);
10321 calibrator.getInitialBiasAsMatrix(b2);
10322 assertEquals(b1, b2);
10323 final Matrix ma1 = calibrator.getInitialMa();
10324 assertEquals(ma1, new Matrix(3, 3));
10325 final Matrix ma2 = new Matrix(3, 3);
10326 calibrator.getInitialMa(ma2);
10327 assertEquals(ma1, ma2);
10328 assertSame(calibrator.getEcefPosition(), ecefPosition);
10329 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10330 final NEDPosition nedPosition1 = new NEDPosition();
10331 assertTrue(calibrator.getNedPosition(nedPosition1));
10332 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10333 assertNull(calibrator.getMeasurements());
10334 assertFalse(calibrator.isCommonAxisUsed());
10335 assertSame(calibrator.getListener(), this);
10336 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10337 assertFalse(calibrator.isReady());
10338 assertFalse(calibrator.isRunning());
10339 assertNull(calibrator.getEstimatedBiases());
10340 assertFalse(calibrator.getEstimatedBiases(null));
10341 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10342 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10343 assertNull(calibrator.getEstimatedBiasFx());
10344 assertNull(calibrator.getEstimatedBiasFy());
10345 assertNull(calibrator.getEstimatedBiasFz());
10346 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10347 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10348 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10349 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10350 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10351 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10352 assertNull(calibrator.getEstimatedMa());
10353 assertNull(calibrator.getEstimatedSx());
10354 assertNull(calibrator.getEstimatedSy());
10355 assertNull(calibrator.getEstimatedSz());
10356 assertNull(calibrator.getEstimatedMxy());
10357 assertNull(calibrator.getEstimatedMxz());
10358 assertNull(calibrator.getEstimatedMyx());
10359 assertNull(calibrator.getEstimatedMyz());
10360 assertNull(calibrator.getEstimatedMzx());
10361 assertNull(calibrator.getEstimatedMzy());
10362 assertNull(calibrator.getEstimatedCovariance());
10363 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10364 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10365 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10366 assertNotNull(calibrator.getGroundTruthGravityNorm());
10367 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10368 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10369 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10370 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10371 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10372 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10373 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10374 }
10375
10376 @Test
10377 public void testConstructor89() throws WrongSizeException {
10378 final Collection<StandardDeviationBodyKinematics> measurements =
10379 Collections.emptyList();
10380
10381 final Matrix ba = generateBa();
10382 final double biasX = ba.getElementAtIndex(0);
10383 final double biasY = ba.getElementAtIndex(1);
10384 final double biasZ = ba.getElementAtIndex(2);
10385
10386 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10387 final double latitude = Math.toRadians(
10388 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10389 final double longitude = Math.toRadians(
10390 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10391 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10392 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10393 final NEDVelocity nedVelocity = new NEDVelocity();
10394 final ECEFPosition ecefPosition = new ECEFPosition();
10395 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10396 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10397 ecefPosition, ecefVelocity);
10398
10399 final KnownPositionAccelerometerCalibrator calibrator =
10400 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10401 biasX, biasY, biasZ);
10402
10403
10404 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10405 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10406 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10407 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10408 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10409 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10410 final Acceleration bx2 = new Acceleration(0.0,
10411 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10412 calibrator.getInitialBiasXAsAcceleration(bx2);
10413 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10414 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10415 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10416 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10417 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10418 final Acceleration by2 = new Acceleration(0.0,
10419 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10420 calibrator.getInitialBiasYAsAcceleration(by2);
10421 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10422 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10423 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10424 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10425 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10426 final Acceleration bz2 = new Acceleration(0.0,
10427 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10428 calibrator.getInitialBiasZAsAcceleration(bz2);
10429 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10430 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10431 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10432 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10433 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10434 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10435 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10436 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10437 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10438 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10439 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10440 final double[] bias1 = calibrator.getInitialBias();
10441 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10442 final double[] bias2 = new double[3];
10443 calibrator.getInitialBias(bias2);
10444 assertArrayEquals(bias1, bias2, 0.0);
10445 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10446 assertEquals(b1, ba);
10447 final Matrix b2 = new Matrix(3, 1);
10448 calibrator.getInitialBiasAsMatrix(b2);
10449 assertEquals(b1, b2);
10450 final Matrix ma1 = calibrator.getInitialMa();
10451 assertEquals(ma1, new Matrix(3, 3));
10452 final Matrix ma2 = new Matrix(3, 3);
10453 calibrator.getInitialMa(ma2);
10454 assertEquals(ma1, ma2);
10455 assertSame(calibrator.getEcefPosition(), ecefPosition);
10456 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10457 final NEDPosition nedPosition1 = new NEDPosition();
10458 assertTrue(calibrator.getNedPosition(nedPosition1));
10459 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10460 assertSame(calibrator.getMeasurements(), measurements);
10461 assertFalse(calibrator.isCommonAxisUsed());
10462 assertNull(calibrator.getListener());
10463 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10464 assertFalse(calibrator.isReady());
10465 assertFalse(calibrator.isRunning());
10466 assertNull(calibrator.getEstimatedBiases());
10467 assertFalse(calibrator.getEstimatedBiases(null));
10468 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10469 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10470 assertNull(calibrator.getEstimatedBiasFx());
10471 assertNull(calibrator.getEstimatedBiasFy());
10472 assertNull(calibrator.getEstimatedBiasFz());
10473 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10474 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10475 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10476 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10477 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10478 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10479 assertNull(calibrator.getEstimatedMa());
10480 assertNull(calibrator.getEstimatedSx());
10481 assertNull(calibrator.getEstimatedSy());
10482 assertNull(calibrator.getEstimatedSz());
10483 assertNull(calibrator.getEstimatedMxy());
10484 assertNull(calibrator.getEstimatedMxz());
10485 assertNull(calibrator.getEstimatedMyx());
10486 assertNull(calibrator.getEstimatedMyz());
10487 assertNull(calibrator.getEstimatedMzx());
10488 assertNull(calibrator.getEstimatedMzy());
10489 assertNull(calibrator.getEstimatedCovariance());
10490 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10491 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10492 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10493 assertNotNull(calibrator.getGroundTruthGravityNorm());
10494 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10495 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10496 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10497 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10498 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10499 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10500 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10501 }
10502
10503 @Test
10504 public void testConstructor90() throws WrongSizeException {
10505 final Collection<StandardDeviationBodyKinematics> measurements =
10506 Collections.emptyList();
10507
10508 final Matrix ba = generateBa();
10509 final double biasX = ba.getElementAtIndex(0);
10510 final double biasY = ba.getElementAtIndex(1);
10511 final double biasZ = ba.getElementAtIndex(2);
10512
10513 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10514 final double latitude = Math.toRadians(
10515 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10516 final double longitude = Math.toRadians(
10517 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10518 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10519 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10520 final NEDVelocity nedVelocity = new NEDVelocity();
10521 final ECEFPosition ecefPosition = new ECEFPosition();
10522 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10523 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10524 ecefPosition, ecefVelocity);
10525
10526 final KnownPositionAccelerometerCalibrator calibrator =
10527 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10528 biasX, biasY, biasZ, this);
10529
10530
10531 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10532 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10533 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10534 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10535 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10536 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10537 final Acceleration bx2 = new Acceleration(0.0,
10538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10539 calibrator.getInitialBiasXAsAcceleration(bx2);
10540 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10541 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10542 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10543 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10544 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10545 final Acceleration by2 = new Acceleration(0.0,
10546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10547 calibrator.getInitialBiasYAsAcceleration(by2);
10548 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10549 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10550 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10551 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10552 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10553 final Acceleration bz2 = new Acceleration(0.0,
10554 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10555 calibrator.getInitialBiasZAsAcceleration(bz2);
10556 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10557 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10558 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10559 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10560 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10561 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10562 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10563 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10564 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10565 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10566 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10567 final double[] bias1 = calibrator.getInitialBias();
10568 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10569 final double[] bias2 = new double[3];
10570 calibrator.getInitialBias(bias2);
10571 assertArrayEquals(bias1, bias2, 0.0);
10572 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10573 assertEquals(b1, ba);
10574 final Matrix b2 = new Matrix(3, 1);
10575 calibrator.getInitialBiasAsMatrix(b2);
10576 assertEquals(b1, b2);
10577 final Matrix ma1 = calibrator.getInitialMa();
10578 assertEquals(ma1, new Matrix(3, 3));
10579 final Matrix ma2 = new Matrix(3, 3);
10580 calibrator.getInitialMa(ma2);
10581 assertEquals(ma1, ma2);
10582 assertSame(calibrator.getEcefPosition(), ecefPosition);
10583 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10584 final NEDPosition nedPosition1 = new NEDPosition();
10585 assertTrue(calibrator.getNedPosition(nedPosition1));
10586 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10587 assertSame(calibrator.getMeasurements(), measurements);
10588 assertFalse(calibrator.isCommonAxisUsed());
10589 assertSame(calibrator.getListener(), this);
10590 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
10591 assertFalse(calibrator.isReady());
10592 assertFalse(calibrator.isRunning());
10593 assertNull(calibrator.getEstimatedBiases());
10594 assertFalse(calibrator.getEstimatedBiases(null));
10595 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10596 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10597 assertNull(calibrator.getEstimatedBiasFx());
10598 assertNull(calibrator.getEstimatedBiasFy());
10599 assertNull(calibrator.getEstimatedBiasFz());
10600 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10601 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10602 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10603 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10604 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10605 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10606 assertNull(calibrator.getEstimatedMa());
10607 assertNull(calibrator.getEstimatedSx());
10608 assertNull(calibrator.getEstimatedSy());
10609 assertNull(calibrator.getEstimatedSz());
10610 assertNull(calibrator.getEstimatedMxy());
10611 assertNull(calibrator.getEstimatedMxz());
10612 assertNull(calibrator.getEstimatedMyx());
10613 assertNull(calibrator.getEstimatedMyz());
10614 assertNull(calibrator.getEstimatedMzx());
10615 assertNull(calibrator.getEstimatedMzy());
10616 assertNull(calibrator.getEstimatedCovariance());
10617 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10618 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10619 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10620 assertNotNull(calibrator.getGroundTruthGravityNorm());
10621 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10622 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10623 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10624 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10625 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10626 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10627 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10628 }
10629
10630 @Test
10631 public void testConstructor91() throws WrongSizeException {
10632 final Matrix ba = generateBa();
10633 final double biasX = ba.getElementAtIndex(0);
10634 final double biasY = ba.getElementAtIndex(1);
10635 final double biasZ = ba.getElementAtIndex(2);
10636
10637 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10638 final double latitude = Math.toRadians(
10639 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10640 final double longitude = Math.toRadians(
10641 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10642 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10643 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10644 final NEDVelocity nedVelocity = new NEDVelocity();
10645 final ECEFPosition ecefPosition = new ECEFPosition();
10646 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10647 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10648 ecefPosition, ecefVelocity);
10649
10650 final KnownPositionAccelerometerCalibrator calibrator =
10651 new KnownPositionAccelerometerCalibrator(ecefPosition,
10652 true, biasX, biasY, biasZ);
10653
10654
10655 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10656 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10657 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10658 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10659 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10660 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10661 final Acceleration bx2 = new Acceleration(0.0,
10662 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10663 calibrator.getInitialBiasXAsAcceleration(bx2);
10664 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10665 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10666 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10667 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10668 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10669 final Acceleration by2 = new Acceleration(0.0,
10670 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10671 calibrator.getInitialBiasYAsAcceleration(by2);
10672 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10673 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10674 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10675 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10676 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10677 final Acceleration bz2 = new Acceleration(0.0,
10678 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10679 calibrator.getInitialBiasZAsAcceleration(bz2);
10680 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10681 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10682 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10683 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10684 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10685 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10686 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10687 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10688 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10689 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10690 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10691 final double[] bias1 = calibrator.getInitialBias();
10692 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10693 final double[] bias2 = new double[3];
10694 calibrator.getInitialBias(bias2);
10695 assertArrayEquals(bias1, bias2, 0.0);
10696 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10697 assertEquals(b1, ba);
10698 final Matrix b2 = new Matrix(3, 1);
10699 calibrator.getInitialBiasAsMatrix(b2);
10700 assertEquals(b1, b2);
10701 final Matrix ma1 = calibrator.getInitialMa();
10702 assertEquals(ma1, new Matrix(3, 3));
10703 final Matrix ma2 = new Matrix(3, 3);
10704 calibrator.getInitialMa(ma2);
10705 assertEquals(ma1, ma2);
10706 assertSame(calibrator.getEcefPosition(), ecefPosition);
10707 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10708 final NEDPosition nedPosition1 = new NEDPosition();
10709 assertTrue(calibrator.getNedPosition(nedPosition1));
10710 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10711 assertNull(calibrator.getMeasurements());
10712 assertTrue(calibrator.isCommonAxisUsed());
10713 assertNull(calibrator.getListener());
10714 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10715 assertFalse(calibrator.isReady());
10716 assertFalse(calibrator.isRunning());
10717 assertNull(calibrator.getEstimatedBiases());
10718 assertFalse(calibrator.getEstimatedBiases(null));
10719 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10720 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10721 assertNull(calibrator.getEstimatedBiasFx());
10722 assertNull(calibrator.getEstimatedBiasFy());
10723 assertNull(calibrator.getEstimatedBiasFz());
10724 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10725 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10726 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10727 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10728 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10729 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10730 assertNull(calibrator.getEstimatedMa());
10731 assertNull(calibrator.getEstimatedSx());
10732 assertNull(calibrator.getEstimatedSy());
10733 assertNull(calibrator.getEstimatedSz());
10734 assertNull(calibrator.getEstimatedMxy());
10735 assertNull(calibrator.getEstimatedMxz());
10736 assertNull(calibrator.getEstimatedMyx());
10737 assertNull(calibrator.getEstimatedMyz());
10738 assertNull(calibrator.getEstimatedMzx());
10739 assertNull(calibrator.getEstimatedMzy());
10740 assertNull(calibrator.getEstimatedCovariance());
10741 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10742 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10743 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10744 assertNotNull(calibrator.getGroundTruthGravityNorm());
10745 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10746 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10747 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10748 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10749 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10750 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10751 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10752 }
10753
10754 @Test
10755 public void testConstructor92() throws WrongSizeException {
10756 final Matrix ba = generateBa();
10757 final double biasX = ba.getElementAtIndex(0);
10758 final double biasY = ba.getElementAtIndex(1);
10759 final double biasZ = ba.getElementAtIndex(2);
10760
10761 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10762 final double latitude = Math.toRadians(
10763 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10764 final double longitude = Math.toRadians(
10765 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10766 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10767 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10768 final NEDVelocity nedVelocity = new NEDVelocity();
10769 final ECEFPosition ecefPosition = new ECEFPosition();
10770 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10771 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10772 ecefPosition, ecefVelocity);
10773
10774 final KnownPositionAccelerometerCalibrator calibrator =
10775 new KnownPositionAccelerometerCalibrator(ecefPosition,
10776 true, biasX, biasY, biasZ, this);
10777
10778
10779 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10780 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10781 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10782 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10783 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10784 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10785 final Acceleration bx2 = new Acceleration(0.0,
10786 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10787 calibrator.getInitialBiasXAsAcceleration(bx2);
10788 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10789 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10790 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10791 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10792 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10793 final Acceleration by2 = new Acceleration(0.0,
10794 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10795 calibrator.getInitialBiasYAsAcceleration(by2);
10796 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10797 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10798 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10799 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10800 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10801 final Acceleration bz2 = new Acceleration(0.0,
10802 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10803 calibrator.getInitialBiasZAsAcceleration(bz2);
10804 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10805 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10806 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10807 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10808 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10809 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10810 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10811 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10812 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10813 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10814 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10815 final double[] bias1 = calibrator.getInitialBias();
10816 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10817 final double[] bias2 = new double[3];
10818 calibrator.getInitialBias(bias2);
10819 assertArrayEquals(bias1, bias2, 0.0);
10820 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10821 assertEquals(b1, ba);
10822 final Matrix b2 = new Matrix(3, 1);
10823 calibrator.getInitialBiasAsMatrix(b2);
10824 assertEquals(b1, b2);
10825 final Matrix ma1 = calibrator.getInitialMa();
10826 assertEquals(ma1, new Matrix(3, 3));
10827 final Matrix ma2 = new Matrix(3, 3);
10828 calibrator.getInitialMa(ma2);
10829 assertEquals(ma1, ma2);
10830 assertSame(calibrator.getEcefPosition(), ecefPosition);
10831 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10832 final NEDPosition nedPosition1 = new NEDPosition();
10833 assertTrue(calibrator.getNedPosition(nedPosition1));
10834 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10835 assertNull(calibrator.getMeasurements());
10836 assertTrue(calibrator.isCommonAxisUsed());
10837 assertSame(calibrator.getListener(), this);
10838 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10839 assertFalse(calibrator.isReady());
10840 assertFalse(calibrator.isRunning());
10841 assertNull(calibrator.getEstimatedBiases());
10842 assertFalse(calibrator.getEstimatedBiases(null));
10843 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10844 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10845 assertNull(calibrator.getEstimatedBiasFx());
10846 assertNull(calibrator.getEstimatedBiasFy());
10847 assertNull(calibrator.getEstimatedBiasFz());
10848 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10849 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10850 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10851 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10852 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10853 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10854 assertNull(calibrator.getEstimatedMa());
10855 assertNull(calibrator.getEstimatedSx());
10856 assertNull(calibrator.getEstimatedSy());
10857 assertNull(calibrator.getEstimatedSz());
10858 assertNull(calibrator.getEstimatedMxy());
10859 assertNull(calibrator.getEstimatedMxz());
10860 assertNull(calibrator.getEstimatedMyx());
10861 assertNull(calibrator.getEstimatedMyz());
10862 assertNull(calibrator.getEstimatedMzx());
10863 assertNull(calibrator.getEstimatedMzy());
10864 assertNull(calibrator.getEstimatedCovariance());
10865 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10866 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10867 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10868 assertNotNull(calibrator.getGroundTruthGravityNorm());
10869 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10870 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10871 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10872 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
10873 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
10874 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
10875 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
10876 }
10877
10878 @Test
10879 public void testConstructor93() throws WrongSizeException {
10880 final Collection<StandardDeviationBodyKinematics> measurements =
10881 Collections.emptyList();
10882
10883 final Matrix ba = generateBa();
10884 final double biasX = ba.getElementAtIndex(0);
10885 final double biasY = ba.getElementAtIndex(1);
10886 final double biasZ = ba.getElementAtIndex(2);
10887
10888 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
10889 final double latitude = Math.toRadians(
10890 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
10891 final double longitude = Math.toRadians(
10892 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
10893 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
10894 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
10895 final NEDVelocity nedVelocity = new NEDVelocity();
10896 final ECEFPosition ecefPosition = new ECEFPosition();
10897 final ECEFVelocity ecefVelocity = new ECEFVelocity();
10898 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
10899 ecefPosition, ecefVelocity);
10900
10901 final KnownPositionAccelerometerCalibrator calibrator =
10902 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
10903 true, biasX, biasY, biasZ);
10904
10905
10906 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
10907 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
10908 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
10909 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
10910 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
10911 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10912 final Acceleration bx2 = new Acceleration(0.0,
10913 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10914 calibrator.getInitialBiasXAsAcceleration(bx2);
10915 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
10916 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10917 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
10918 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
10919 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10920 final Acceleration by2 = new Acceleration(0.0,
10921 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10922 calibrator.getInitialBiasYAsAcceleration(by2);
10923 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
10924 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10925 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
10926 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
10927 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10928 final Acceleration bz2 = new Acceleration(0.0,
10929 AccelerationUnit.FEET_PER_SQUARED_SECOND);
10930 calibrator.getInitialBiasZAsAcceleration(bz2);
10931 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
10932 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
10933 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
10934 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
10935 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
10936 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
10937 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
10938 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
10939 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
10940 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
10941 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
10942 final double[] bias1 = calibrator.getInitialBias();
10943 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
10944 final double[] bias2 = new double[3];
10945 calibrator.getInitialBias(bias2);
10946 assertArrayEquals(bias1, bias2, 0.0);
10947 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
10948 assertEquals(b1, ba);
10949 final Matrix b2 = new Matrix(3, 1);
10950 calibrator.getInitialBiasAsMatrix(b2);
10951 assertEquals(b1, b2);
10952 final Matrix ma1 = calibrator.getInitialMa();
10953 assertEquals(ma1, new Matrix(3, 3));
10954 final Matrix ma2 = new Matrix(3, 3);
10955 calibrator.getInitialMa(ma2);
10956 assertEquals(ma1, ma2);
10957 assertSame(calibrator.getEcefPosition(), ecefPosition);
10958 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
10959 final NEDPosition nedPosition1 = new NEDPosition();
10960 assertTrue(calibrator.getNedPosition(nedPosition1));
10961 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
10962 assertSame(calibrator.getMeasurements(), measurements);
10963 assertTrue(calibrator.isCommonAxisUsed());
10964 assertNull(calibrator.getListener());
10965 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
10966 assertFalse(calibrator.isReady());
10967 assertFalse(calibrator.isRunning());
10968 assertNull(calibrator.getEstimatedBiases());
10969 assertFalse(calibrator.getEstimatedBiases(null));
10970 assertNull(calibrator.getEstimatedBiasesAsMatrix());
10971 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
10972 assertNull(calibrator.getEstimatedBiasFx());
10973 assertNull(calibrator.getEstimatedBiasFy());
10974 assertNull(calibrator.getEstimatedBiasFz());
10975 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
10976 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
10977 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
10978 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
10979 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
10980 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
10981 assertNull(calibrator.getEstimatedMa());
10982 assertNull(calibrator.getEstimatedSx());
10983 assertNull(calibrator.getEstimatedSy());
10984 assertNull(calibrator.getEstimatedSz());
10985 assertNull(calibrator.getEstimatedMxy());
10986 assertNull(calibrator.getEstimatedMxz());
10987 assertNull(calibrator.getEstimatedMyx());
10988 assertNull(calibrator.getEstimatedMyz());
10989 assertNull(calibrator.getEstimatedMzx());
10990 assertNull(calibrator.getEstimatedMzy());
10991 assertNull(calibrator.getEstimatedCovariance());
10992 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
10993 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
10994 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
10995 assertNotNull(calibrator.getGroundTruthGravityNorm());
10996 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
10997 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
10998 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
10999 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11000 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11001 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11002 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11003 }
11004
11005 @Test
11006 public void testConstructor94() throws WrongSizeException {
11007 final Collection<StandardDeviationBodyKinematics> measurements =
11008 Collections.emptyList();
11009
11010 final Matrix ba = generateBa();
11011 final double biasX = ba.getElementAtIndex(0);
11012 final double biasY = ba.getElementAtIndex(1);
11013 final double biasZ = ba.getElementAtIndex(2);
11014
11015 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11016 final double latitude = Math.toRadians(
11017 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11018 final double longitude = Math.toRadians(
11019 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11020 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11021 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11022 final NEDVelocity nedVelocity = new NEDVelocity();
11023 final ECEFPosition ecefPosition = new ECEFPosition();
11024 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11025 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11026 ecefPosition, ecefVelocity);
11027
11028 final KnownPositionAccelerometerCalibrator calibrator =
11029 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11030 true, biasX, biasY, biasZ, this);
11031
11032
11033 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11034 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11035 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11036 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11037 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11038 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11039 final Acceleration bx2 = new Acceleration(0.0,
11040 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11041 calibrator.getInitialBiasXAsAcceleration(bx2);
11042 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11043 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11044 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11045 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11046 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11047 final Acceleration by2 = new Acceleration(0.0,
11048 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11049 calibrator.getInitialBiasYAsAcceleration(by2);
11050 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11051 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11052 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11053 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11054 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11055 final Acceleration bz2 = new Acceleration(0.0,
11056 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11057 calibrator.getInitialBiasZAsAcceleration(bz2);
11058 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11059 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11060 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11061 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11062 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11063 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11064 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11065 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11066 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11067 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11068 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11069 final double[] bias1 = calibrator.getInitialBias();
11070 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11071 final double[] bias2 = new double[3];
11072 calibrator.getInitialBias(bias2);
11073 assertArrayEquals(bias1, bias2, 0.0);
11074 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11075 assertEquals(b1, ba);
11076 final Matrix b2 = new Matrix(3, 1);
11077 calibrator.getInitialBiasAsMatrix(b2);
11078 assertEquals(b1, b2);
11079 final Matrix ma1 = calibrator.getInitialMa();
11080 assertEquals(ma1, new Matrix(3, 3));
11081 final Matrix ma2 = new Matrix(3, 3);
11082 calibrator.getInitialMa(ma2);
11083 assertEquals(ma1, ma2);
11084 assertSame(calibrator.getEcefPosition(), ecefPosition);
11085 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11086 final NEDPosition nedPosition1 = new NEDPosition();
11087 assertTrue(calibrator.getNedPosition(nedPosition1));
11088 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11089 assertSame(calibrator.getMeasurements(), measurements);
11090 assertTrue(calibrator.isCommonAxisUsed());
11091 assertSame(calibrator.getListener(), this);
11092 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11093 assertFalse(calibrator.isReady());
11094 assertFalse(calibrator.isRunning());
11095 assertNull(calibrator.getEstimatedBiases());
11096 assertFalse(calibrator.getEstimatedBiases(null));
11097 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11098 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11099 assertNull(calibrator.getEstimatedBiasFx());
11100 assertNull(calibrator.getEstimatedBiasFy());
11101 assertNull(calibrator.getEstimatedBiasFz());
11102 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11103 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11104 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11105 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11106 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11107 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11108 assertNull(calibrator.getEstimatedMa());
11109 assertNull(calibrator.getEstimatedSx());
11110 assertNull(calibrator.getEstimatedSy());
11111 assertNull(calibrator.getEstimatedSz());
11112 assertNull(calibrator.getEstimatedMxy());
11113 assertNull(calibrator.getEstimatedMxz());
11114 assertNull(calibrator.getEstimatedMyx());
11115 assertNull(calibrator.getEstimatedMyz());
11116 assertNull(calibrator.getEstimatedMzx());
11117 assertNull(calibrator.getEstimatedMzy());
11118 assertNull(calibrator.getEstimatedCovariance());
11119 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11120 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11121 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11122 assertNotNull(calibrator.getGroundTruthGravityNorm());
11123 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11124 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11125 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11126 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11127 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11128 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11129 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11130 }
11131
11132 @Test
11133 public void testConstructor95() throws WrongSizeException {
11134 final Matrix ba = generateBa();
11135 final double biasX = ba.getElementAtIndex(0);
11136 final double biasY = ba.getElementAtIndex(1);
11137 final double biasZ = ba.getElementAtIndex(2);
11138
11139 final Acceleration bx = new Acceleration(biasX,
11140 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11141 final Acceleration by = new Acceleration(biasY,
11142 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11143 final Acceleration bz = new Acceleration(biasZ,
11144 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11145
11146 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11147 final double latitude = Math.toRadians(
11148 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11149 final double longitude = Math.toRadians(
11150 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11151 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11152 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11153 final NEDVelocity nedVelocity = new NEDVelocity();
11154 final ECEFPosition ecefPosition = new ECEFPosition();
11155 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11156 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11157 ecefPosition, ecefVelocity);
11158
11159 final KnownPositionAccelerometerCalibrator calibrator =
11160 new KnownPositionAccelerometerCalibrator(ecefPosition,
11161 bx, by, bz);
11162
11163
11164 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11165 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11166 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11167 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11168 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11169 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11170 final Acceleration bx2 = new Acceleration(0.0,
11171 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11172 calibrator.getInitialBiasXAsAcceleration(bx2);
11173 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11174 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11175 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11176 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11177 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11178 final Acceleration by2 = new Acceleration(0.0,
11179 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11180 calibrator.getInitialBiasYAsAcceleration(by2);
11181 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11182 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11183 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11184 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11185 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11186 final Acceleration bz2 = new Acceleration(0.0,
11187 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11188 calibrator.getInitialBiasZAsAcceleration(bz2);
11189 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11190 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11191 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11192 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11193 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11194 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11195 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11196 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11197 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11198 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11199 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11200 final double[] bias1 = calibrator.getInitialBias();
11201 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11202 final double[] bias2 = new double[3];
11203 calibrator.getInitialBias(bias2);
11204 assertArrayEquals(bias1, bias2, 0.0);
11205 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11206 assertEquals(b1, ba);
11207 final Matrix b2 = new Matrix(3, 1);
11208 calibrator.getInitialBiasAsMatrix(b2);
11209 assertEquals(b1, b2);
11210 final Matrix ma1 = calibrator.getInitialMa();
11211 assertEquals(ma1, new Matrix(3, 3));
11212 final Matrix ma2 = new Matrix(3, 3);
11213 calibrator.getInitialMa(ma2);
11214 assertEquals(ma1, ma2);
11215 assertSame(calibrator.getEcefPosition(), ecefPosition);
11216 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11217 final NEDPosition nedPosition1 = new NEDPosition();
11218 assertTrue(calibrator.getNedPosition(nedPosition1));
11219 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11220 assertNull(calibrator.getMeasurements());
11221 assertFalse(calibrator.isCommonAxisUsed());
11222 assertNull(calibrator.getListener());
11223 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11224 assertFalse(calibrator.isReady());
11225 assertFalse(calibrator.isRunning());
11226 assertNull(calibrator.getEstimatedBiases());
11227 assertFalse(calibrator.getEstimatedBiases(null));
11228 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11229 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11230 assertNull(calibrator.getEstimatedBiasFx());
11231 assertNull(calibrator.getEstimatedBiasFy());
11232 assertNull(calibrator.getEstimatedBiasFz());
11233 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11234 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11235 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11236 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11237 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11238 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11239 assertNull(calibrator.getEstimatedMa());
11240 assertNull(calibrator.getEstimatedSx());
11241 assertNull(calibrator.getEstimatedSy());
11242 assertNull(calibrator.getEstimatedSz());
11243 assertNull(calibrator.getEstimatedMxy());
11244 assertNull(calibrator.getEstimatedMxz());
11245 assertNull(calibrator.getEstimatedMyx());
11246 assertNull(calibrator.getEstimatedMyz());
11247 assertNull(calibrator.getEstimatedMzx());
11248 assertNull(calibrator.getEstimatedMzy());
11249 assertNull(calibrator.getEstimatedCovariance());
11250 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11251 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11252 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11253 assertNotNull(calibrator.getGroundTruthGravityNorm());
11254 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11255 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11256 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11257 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11258 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11259 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11260 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11261 }
11262
11263 @Test
11264 public void testConstructor96() throws WrongSizeException {
11265 final Matrix ba = generateBa();
11266 final double biasX = ba.getElementAtIndex(0);
11267 final double biasY = ba.getElementAtIndex(1);
11268 final double biasZ = ba.getElementAtIndex(2);
11269
11270 final Acceleration bx = new Acceleration(biasX,
11271 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11272 final Acceleration by = new Acceleration(biasY,
11273 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11274 final Acceleration bz = new Acceleration(biasZ,
11275 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11276
11277 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11278 final double latitude = Math.toRadians(
11279 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11280 final double longitude = Math.toRadians(
11281 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11282 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11283 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11284 final NEDVelocity nedVelocity = new NEDVelocity();
11285 final ECEFPosition ecefPosition = new ECEFPosition();
11286 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11287 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11288 ecefPosition, ecefVelocity);
11289
11290 final KnownPositionAccelerometerCalibrator calibrator =
11291 new KnownPositionAccelerometerCalibrator(ecefPosition,
11292 bx, by, bz, this);
11293
11294
11295 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11296 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11297 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11298 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11299 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11300 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11301 final Acceleration bx2 = new Acceleration(0.0,
11302 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11303 calibrator.getInitialBiasXAsAcceleration(bx2);
11304 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11305 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11306 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11307 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11308 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11309 final Acceleration by2 = new Acceleration(0.0,
11310 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11311 calibrator.getInitialBiasYAsAcceleration(by2);
11312 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11313 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11314 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11315 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11316 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11317 final Acceleration bz2 = new Acceleration(0.0,
11318 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11319 calibrator.getInitialBiasZAsAcceleration(bz2);
11320 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11321 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11322 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11323 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11324 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11325 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11326 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11327 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11328 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11329 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11330 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11331 final double[] bias1 = calibrator.getInitialBias();
11332 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11333 final double[] bias2 = new double[3];
11334 calibrator.getInitialBias(bias2);
11335 assertArrayEquals(bias1, bias2, 0.0);
11336 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11337 assertEquals(b1, ba);
11338 final Matrix b2 = new Matrix(3, 1);
11339 calibrator.getInitialBiasAsMatrix(b2);
11340 assertEquals(b1, b2);
11341 final Matrix ma1 = calibrator.getInitialMa();
11342 assertEquals(ma1, new Matrix(3, 3));
11343 final Matrix ma2 = new Matrix(3, 3);
11344 calibrator.getInitialMa(ma2);
11345 assertEquals(ma1, ma2);
11346 assertSame(calibrator.getEcefPosition(), ecefPosition);
11347 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11348 final NEDPosition nedPosition1 = new NEDPosition();
11349 assertTrue(calibrator.getNedPosition(nedPosition1));
11350 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11351 assertNull(calibrator.getMeasurements());
11352 assertFalse(calibrator.isCommonAxisUsed());
11353 assertSame(calibrator.getListener(), this);
11354 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11355 assertFalse(calibrator.isReady());
11356 assertFalse(calibrator.isRunning());
11357 assertNull(calibrator.getEstimatedBiases());
11358 assertFalse(calibrator.getEstimatedBiases(null));
11359 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11360 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11361 assertNull(calibrator.getEstimatedBiasFx());
11362 assertNull(calibrator.getEstimatedBiasFy());
11363 assertNull(calibrator.getEstimatedBiasFz());
11364 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11365 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11366 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11367 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11368 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11369 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11370 assertNull(calibrator.getEstimatedMa());
11371 assertNull(calibrator.getEstimatedSx());
11372 assertNull(calibrator.getEstimatedSy());
11373 assertNull(calibrator.getEstimatedSz());
11374 assertNull(calibrator.getEstimatedMxy());
11375 assertNull(calibrator.getEstimatedMxz());
11376 assertNull(calibrator.getEstimatedMyx());
11377 assertNull(calibrator.getEstimatedMyz());
11378 assertNull(calibrator.getEstimatedMzx());
11379 assertNull(calibrator.getEstimatedMzy());
11380 assertNull(calibrator.getEstimatedCovariance());
11381 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11382 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11383 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11384 assertNotNull(calibrator.getGroundTruthGravityNorm());
11385 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11386 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11387 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11388 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11389 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11390 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11391 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11392 }
11393
11394 @Test
11395 public void testConstructor97() throws WrongSizeException {
11396 final Collection<StandardDeviationBodyKinematics> measurements =
11397 Collections.emptyList();
11398
11399 final Matrix ba = generateBa();
11400 final double biasX = ba.getElementAtIndex(0);
11401 final double biasY = ba.getElementAtIndex(1);
11402 final double biasZ = ba.getElementAtIndex(2);
11403
11404 final Acceleration bx = new Acceleration(biasX,
11405 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11406 final Acceleration by = new Acceleration(biasY,
11407 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11408 final Acceleration bz = new Acceleration(biasZ,
11409 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11410
11411 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11412 final double latitude = Math.toRadians(
11413 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11414 final double longitude = Math.toRadians(
11415 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11416 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11417 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11418 final NEDVelocity nedVelocity = new NEDVelocity();
11419 final ECEFPosition ecefPosition = new ECEFPosition();
11420 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11421 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11422 ecefPosition, ecefVelocity);
11423
11424 final KnownPositionAccelerometerCalibrator calibrator =
11425 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11426 bx, by, bz);
11427
11428
11429 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11430 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11431 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11432 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11433 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11434 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11435 final Acceleration bx2 = new Acceleration(0.0,
11436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11437 calibrator.getInitialBiasXAsAcceleration(bx2);
11438 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11439 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11440 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11441 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11442 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11443 final Acceleration by2 = new Acceleration(0.0,
11444 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11445 calibrator.getInitialBiasYAsAcceleration(by2);
11446 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11447 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11448 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11449 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11450 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11451 final Acceleration bz2 = new Acceleration(0.0,
11452 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11453 calibrator.getInitialBiasZAsAcceleration(bz2);
11454 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11455 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11456 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11457 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11458 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11459 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11460 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11461 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11462 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11463 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11464 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11465 final double[] bias1 = calibrator.getInitialBias();
11466 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11467 final double[] bias2 = new double[3];
11468 calibrator.getInitialBias(bias2);
11469 assertArrayEquals(bias1, bias2, 0.0);
11470 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11471 assertEquals(b1, ba);
11472 final Matrix b2 = new Matrix(3, 1);
11473 calibrator.getInitialBiasAsMatrix(b2);
11474 assertEquals(b1, b2);
11475 final Matrix ma1 = calibrator.getInitialMa();
11476 assertEquals(ma1, new Matrix(3, 3));
11477 final Matrix ma2 = new Matrix(3, 3);
11478 calibrator.getInitialMa(ma2);
11479 assertEquals(ma1, ma2);
11480 assertSame(calibrator.getEcefPosition(), ecefPosition);
11481 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11482 final NEDPosition nedPosition1 = new NEDPosition();
11483 assertTrue(calibrator.getNedPosition(nedPosition1));
11484 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11485 assertSame(calibrator.getMeasurements(), measurements);
11486 assertFalse(calibrator.isCommonAxisUsed());
11487 assertNull(calibrator.getListener());
11488 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11489 assertFalse(calibrator.isReady());
11490 assertFalse(calibrator.isRunning());
11491 assertNull(calibrator.getEstimatedBiases());
11492 assertFalse(calibrator.getEstimatedBiases(null));
11493 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11494 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11495 assertNull(calibrator.getEstimatedBiasFx());
11496 assertNull(calibrator.getEstimatedBiasFy());
11497 assertNull(calibrator.getEstimatedBiasFz());
11498 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11499 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11500 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11501 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11502 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11503 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11504 assertNull(calibrator.getEstimatedMa());
11505 assertNull(calibrator.getEstimatedSx());
11506 assertNull(calibrator.getEstimatedSy());
11507 assertNull(calibrator.getEstimatedSz());
11508 assertNull(calibrator.getEstimatedMxy());
11509 assertNull(calibrator.getEstimatedMxz());
11510 assertNull(calibrator.getEstimatedMyx());
11511 assertNull(calibrator.getEstimatedMyz());
11512 assertNull(calibrator.getEstimatedMzx());
11513 assertNull(calibrator.getEstimatedMzy());
11514 assertNull(calibrator.getEstimatedCovariance());
11515 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11516 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11517 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11518 assertNotNull(calibrator.getGroundTruthGravityNorm());
11519 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11520 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11521 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11522 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11523 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11524 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11525 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11526 }
11527
11528 @Test
11529 public void testConstructor98() throws WrongSizeException {
11530 final Collection<StandardDeviationBodyKinematics> measurements =
11531 Collections.emptyList();
11532
11533 final Matrix ba = generateBa();
11534 final double biasX = ba.getElementAtIndex(0);
11535 final double biasY = ba.getElementAtIndex(1);
11536 final double biasZ = ba.getElementAtIndex(2);
11537
11538 final Acceleration bx = new Acceleration(biasX,
11539 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11540 final Acceleration by = new Acceleration(biasY,
11541 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11542 final Acceleration bz = new Acceleration(biasZ,
11543 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11544
11545 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11546 final double latitude = Math.toRadians(
11547 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11548 final double longitude = Math.toRadians(
11549 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11550 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11551 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11552 final NEDVelocity nedVelocity = new NEDVelocity();
11553 final ECEFPosition ecefPosition = new ECEFPosition();
11554 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11555 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11556 ecefPosition, ecefVelocity);
11557
11558 final KnownPositionAccelerometerCalibrator calibrator =
11559 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11560 bx, by, bz, this);
11561
11562
11563 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11564 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11565 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11566 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11567 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11568 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11569 final Acceleration bx2 = new Acceleration(0.0,
11570 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11571 calibrator.getInitialBiasXAsAcceleration(bx2);
11572 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11573 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11574 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11575 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11576 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11577 final Acceleration by2 = new Acceleration(0.0,
11578 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11579 calibrator.getInitialBiasYAsAcceleration(by2);
11580 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11581 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11582 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11583 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11584 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11585 final Acceleration bz2 = new Acceleration(0.0,
11586 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11587 calibrator.getInitialBiasZAsAcceleration(bz2);
11588 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11589 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11590 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11591 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11592 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11593 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11594 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11595 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11596 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11597 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11598 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11599 final double[] bias1 = calibrator.getInitialBias();
11600 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11601 final double[] bias2 = new double[3];
11602 calibrator.getInitialBias(bias2);
11603 assertArrayEquals(bias1, bias2, 0.0);
11604 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11605 assertEquals(b1, ba);
11606 final Matrix b2 = new Matrix(3, 1);
11607 calibrator.getInitialBiasAsMatrix(b2);
11608 assertEquals(b1, b2);
11609 final Matrix ma1 = calibrator.getInitialMa();
11610 assertEquals(ma1, new Matrix(3, 3));
11611 final Matrix ma2 = new Matrix(3, 3);
11612 calibrator.getInitialMa(ma2);
11613 assertEquals(ma1, ma2);
11614 assertSame(calibrator.getEcefPosition(), ecefPosition);
11615 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11616 final NEDPosition nedPosition1 = new NEDPosition();
11617 assertTrue(calibrator.getNedPosition(nedPosition1));
11618 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11619 assertSame(calibrator.getMeasurements(), measurements);
11620 assertFalse(calibrator.isCommonAxisUsed());
11621 assertSame(calibrator.getListener(), this);
11622 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
11623 assertFalse(calibrator.isReady());
11624 assertFalse(calibrator.isRunning());
11625 assertNull(calibrator.getEstimatedBiases());
11626 assertFalse(calibrator.getEstimatedBiases(null));
11627 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11628 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11629 assertNull(calibrator.getEstimatedBiasFx());
11630 assertNull(calibrator.getEstimatedBiasFy());
11631 assertNull(calibrator.getEstimatedBiasFz());
11632 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11633 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11634 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11635 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11636 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11637 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11638 assertNull(calibrator.getEstimatedMa());
11639 assertNull(calibrator.getEstimatedSx());
11640 assertNull(calibrator.getEstimatedSy());
11641 assertNull(calibrator.getEstimatedSz());
11642 assertNull(calibrator.getEstimatedMxy());
11643 assertNull(calibrator.getEstimatedMxz());
11644 assertNull(calibrator.getEstimatedMyx());
11645 assertNull(calibrator.getEstimatedMyz());
11646 assertNull(calibrator.getEstimatedMzx());
11647 assertNull(calibrator.getEstimatedMzy());
11648 assertNull(calibrator.getEstimatedCovariance());
11649 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11650 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11651 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11652 assertNotNull(calibrator.getGroundTruthGravityNorm());
11653 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11654 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11655 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11656 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11657 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11658 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11659 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11660 }
11661
11662 @Test
11663 public void testConstructor99() throws WrongSizeException {
11664 final Matrix ba = generateBa();
11665 final double biasX = ba.getElementAtIndex(0);
11666 final double biasY = ba.getElementAtIndex(1);
11667 final double biasZ = ba.getElementAtIndex(2);
11668
11669 final Acceleration bx = new Acceleration(biasX,
11670 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11671 final Acceleration by = new Acceleration(biasY,
11672 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11673 final Acceleration bz = new Acceleration(biasZ,
11674 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11675
11676 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11677 final double latitude = Math.toRadians(
11678 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11679 final double longitude = Math.toRadians(
11680 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11681 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11682 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11683 final NEDVelocity nedVelocity = new NEDVelocity();
11684 final ECEFPosition ecefPosition = new ECEFPosition();
11685 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11686 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11687 ecefPosition, ecefVelocity);
11688
11689 final KnownPositionAccelerometerCalibrator calibrator =
11690 new KnownPositionAccelerometerCalibrator(ecefPosition,
11691 true, bx, by, bz);
11692
11693
11694 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11695 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11696 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11697 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11698 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11699 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11700 final Acceleration bx2 = new Acceleration(0.0,
11701 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11702 calibrator.getInitialBiasXAsAcceleration(bx2);
11703 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11704 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11705 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11706 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11707 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11708 final Acceleration by2 = new Acceleration(0.0,
11709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11710 calibrator.getInitialBiasYAsAcceleration(by2);
11711 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11712 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11713 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11714 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11715 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11716 final Acceleration bz2 = new Acceleration(0.0,
11717 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11718 calibrator.getInitialBiasZAsAcceleration(bz2);
11719 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11720 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11721 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11722 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11723 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11724 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11725 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11726 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11727 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11728 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11729 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11730 final double[] bias1 = calibrator.getInitialBias();
11731 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11732 final double[] bias2 = new double[3];
11733 calibrator.getInitialBias(bias2);
11734 assertArrayEquals(bias1, bias2, 0.0);
11735 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11736 assertEquals(b1, ba);
11737 final Matrix b2 = new Matrix(3, 1);
11738 calibrator.getInitialBiasAsMatrix(b2);
11739 assertEquals(b1, b2);
11740 final Matrix ma1 = calibrator.getInitialMa();
11741 assertEquals(ma1, new Matrix(3, 3));
11742 final Matrix ma2 = new Matrix(3, 3);
11743 calibrator.getInitialMa(ma2);
11744 assertEquals(ma1, ma2);
11745 assertSame(calibrator.getEcefPosition(), ecefPosition);
11746 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11747 final NEDPosition nedPosition1 = new NEDPosition();
11748 assertTrue(calibrator.getNedPosition(nedPosition1));
11749 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11750 assertNull(calibrator.getMeasurements());
11751 assertTrue(calibrator.isCommonAxisUsed());
11752 assertNull(calibrator.getListener());
11753 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11754 assertFalse(calibrator.isReady());
11755 assertFalse(calibrator.isRunning());
11756 assertNull(calibrator.getEstimatedBiases());
11757 assertFalse(calibrator.getEstimatedBiases(null));
11758 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11759 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11760 assertNull(calibrator.getEstimatedBiasFx());
11761 assertNull(calibrator.getEstimatedBiasFy());
11762 assertNull(calibrator.getEstimatedBiasFz());
11763 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11764 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11765 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11766 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11767 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11768 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11769 assertNull(calibrator.getEstimatedMa());
11770 assertNull(calibrator.getEstimatedSx());
11771 assertNull(calibrator.getEstimatedSy());
11772 assertNull(calibrator.getEstimatedSz());
11773 assertNull(calibrator.getEstimatedMxy());
11774 assertNull(calibrator.getEstimatedMxz());
11775 assertNull(calibrator.getEstimatedMyx());
11776 assertNull(calibrator.getEstimatedMyz());
11777 assertNull(calibrator.getEstimatedMzx());
11778 assertNull(calibrator.getEstimatedMzy());
11779 assertNull(calibrator.getEstimatedCovariance());
11780 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11781 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11782 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11783 assertNotNull(calibrator.getGroundTruthGravityNorm());
11784 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11785 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11786 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11787 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11788 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11789 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11790 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11791 }
11792
11793 @Test
11794 public void testConstructor100() throws WrongSizeException {
11795 final Matrix ba = generateBa();
11796 final double biasX = ba.getElementAtIndex(0);
11797 final double biasY = ba.getElementAtIndex(1);
11798 final double biasZ = ba.getElementAtIndex(2);
11799
11800 final Acceleration bx = new Acceleration(biasX,
11801 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11802 final Acceleration by = new Acceleration(biasY,
11803 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11804 final Acceleration bz = new Acceleration(biasZ,
11805 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11806
11807 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11808 final double latitude = Math.toRadians(
11809 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11810 final double longitude = Math.toRadians(
11811 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11812 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11813 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11814 final NEDVelocity nedVelocity = new NEDVelocity();
11815 final ECEFPosition ecefPosition = new ECEFPosition();
11816 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11817 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11818 ecefPosition, ecefVelocity);
11819
11820 final KnownPositionAccelerometerCalibrator calibrator =
11821 new KnownPositionAccelerometerCalibrator(ecefPosition,
11822 true, bx, by, bz, this);
11823
11824
11825 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11826 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11827 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11828 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11829 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11830 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11831 final Acceleration bx2 = new Acceleration(0.0,
11832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11833 calibrator.getInitialBiasXAsAcceleration(bx2);
11834 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11835 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11836 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11837 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11838 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11839 final Acceleration by2 = new Acceleration(0.0,
11840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11841 calibrator.getInitialBiasYAsAcceleration(by2);
11842 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11843 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11844 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11845 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11846 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11847 final Acceleration bz2 = new Acceleration(0.0,
11848 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11849 calibrator.getInitialBiasZAsAcceleration(bz2);
11850 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11851 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11852 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11853 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11854 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11855 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11856 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11857 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11858 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11859 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11860 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11861 final double[] bias1 = calibrator.getInitialBias();
11862 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11863 final double[] bias2 = new double[3];
11864 calibrator.getInitialBias(bias2);
11865 assertArrayEquals(bias1, bias2, 0.0);
11866 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
11867 assertEquals(b1, ba);
11868 final Matrix b2 = new Matrix(3, 1);
11869 calibrator.getInitialBiasAsMatrix(b2);
11870 assertEquals(b1, b2);
11871 final Matrix ma1 = calibrator.getInitialMa();
11872 assertEquals(ma1, new Matrix(3, 3));
11873 final Matrix ma2 = new Matrix(3, 3);
11874 calibrator.getInitialMa(ma2);
11875 assertEquals(ma1, ma2);
11876 assertSame(calibrator.getEcefPosition(), ecefPosition);
11877 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
11878 final NEDPosition nedPosition1 = new NEDPosition();
11879 assertTrue(calibrator.getNedPosition(nedPosition1));
11880 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
11881 assertNull(calibrator.getMeasurements());
11882 assertTrue(calibrator.isCommonAxisUsed());
11883 assertSame(calibrator.getListener(), this);
11884 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
11885 assertFalse(calibrator.isReady());
11886 assertFalse(calibrator.isRunning());
11887 assertNull(calibrator.getEstimatedBiases());
11888 assertFalse(calibrator.getEstimatedBiases(null));
11889 assertNull(calibrator.getEstimatedBiasesAsMatrix());
11890 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
11891 assertNull(calibrator.getEstimatedBiasFx());
11892 assertNull(calibrator.getEstimatedBiasFy());
11893 assertNull(calibrator.getEstimatedBiasFz());
11894 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
11895 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
11896 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
11897 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
11898 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
11899 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
11900 assertNull(calibrator.getEstimatedMa());
11901 assertNull(calibrator.getEstimatedSx());
11902 assertNull(calibrator.getEstimatedSy());
11903 assertNull(calibrator.getEstimatedSz());
11904 assertNull(calibrator.getEstimatedMxy());
11905 assertNull(calibrator.getEstimatedMxz());
11906 assertNull(calibrator.getEstimatedMyx());
11907 assertNull(calibrator.getEstimatedMyz());
11908 assertNull(calibrator.getEstimatedMzx());
11909 assertNull(calibrator.getEstimatedMzy());
11910 assertNull(calibrator.getEstimatedCovariance());
11911 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
11912 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
11913 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
11914 assertNotNull(calibrator.getGroundTruthGravityNorm());
11915 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
11916 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
11917 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
11918 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
11919 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
11920 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
11921 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
11922 }
11923
11924 @Test
11925 public void testConstructor101() throws WrongSizeException {
11926 final Collection<StandardDeviationBodyKinematics> measurements =
11927 Collections.emptyList();
11928
11929 final Matrix ba = generateBa();
11930 final double biasX = ba.getElementAtIndex(0);
11931 final double biasY = ba.getElementAtIndex(1);
11932 final double biasZ = ba.getElementAtIndex(2);
11933
11934 final Acceleration bx = new Acceleration(biasX,
11935 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11936 final Acceleration by = new Acceleration(biasY,
11937 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11938 final Acceleration bz = new Acceleration(biasZ,
11939 AccelerationUnit.METERS_PER_SQUARED_SECOND);
11940
11941 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
11942 final double latitude = Math.toRadians(
11943 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
11944 final double longitude = Math.toRadians(
11945 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
11946 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
11947 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
11948 final NEDVelocity nedVelocity = new NEDVelocity();
11949 final ECEFPosition ecefPosition = new ECEFPosition();
11950 final ECEFVelocity ecefVelocity = new ECEFVelocity();
11951 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
11952 ecefPosition, ecefVelocity);
11953
11954 final KnownPositionAccelerometerCalibrator calibrator =
11955 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
11956 true, bx, by, bz);
11957
11958
11959 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
11960 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
11961 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
11962 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
11963 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
11964 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11965 final Acceleration bx2 = new Acceleration(0.0,
11966 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11967 calibrator.getInitialBiasXAsAcceleration(bx2);
11968 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
11969 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11970 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
11971 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
11972 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11973 final Acceleration by2 = new Acceleration(0.0,
11974 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11975 calibrator.getInitialBiasYAsAcceleration(by2);
11976 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
11977 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11978 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
11979 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
11980 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11981 final Acceleration bz2 = new Acceleration(0.0,
11982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
11983 calibrator.getInitialBiasZAsAcceleration(bz2);
11984 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
11985 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
11986 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
11987 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
11988 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
11989 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
11990 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
11991 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
11992 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
11993 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
11994 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
11995 final double[] bias1 = calibrator.getInitialBias();
11996 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
11997 final double[] bias2 = new double[3];
11998 calibrator.getInitialBias(bias2);
11999 assertArrayEquals(bias1, bias2, 0.0);
12000 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12001 assertEquals(b1, ba);
12002 final Matrix b2 = new Matrix(3, 1);
12003 calibrator.getInitialBiasAsMatrix(b2);
12004 assertEquals(b1, b2);
12005 final Matrix ma1 = calibrator.getInitialMa();
12006 assertEquals(ma1, new Matrix(3, 3));
12007 final Matrix ma2 = new Matrix(3, 3);
12008 calibrator.getInitialMa(ma2);
12009 assertEquals(ma1, ma2);
12010 assertSame(calibrator.getEcefPosition(), ecefPosition);
12011 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12012 final NEDPosition nedPosition1 = new NEDPosition();
12013 assertTrue(calibrator.getNedPosition(nedPosition1));
12014 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12015 assertSame(calibrator.getMeasurements(), measurements);
12016 assertTrue(calibrator.isCommonAxisUsed());
12017 assertNull(calibrator.getListener());
12018 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12019 assertFalse(calibrator.isReady());
12020 assertFalse(calibrator.isRunning());
12021 assertNull(calibrator.getEstimatedBiases());
12022 assertFalse(calibrator.getEstimatedBiases(null));
12023 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12024 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12025 assertNull(calibrator.getEstimatedBiasFx());
12026 assertNull(calibrator.getEstimatedBiasFy());
12027 assertNull(calibrator.getEstimatedBiasFz());
12028 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12029 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12030 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12031 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12032 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12033 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12034 assertNull(calibrator.getEstimatedMa());
12035 assertNull(calibrator.getEstimatedSx());
12036 assertNull(calibrator.getEstimatedSy());
12037 assertNull(calibrator.getEstimatedSz());
12038 assertNull(calibrator.getEstimatedMxy());
12039 assertNull(calibrator.getEstimatedMxz());
12040 assertNull(calibrator.getEstimatedMyx());
12041 assertNull(calibrator.getEstimatedMyz());
12042 assertNull(calibrator.getEstimatedMzx());
12043 assertNull(calibrator.getEstimatedMzy());
12044 assertNull(calibrator.getEstimatedCovariance());
12045 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12046 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12047 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12048 assertNotNull(calibrator.getGroundTruthGravityNorm());
12049 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12050 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12051 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12052 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12053 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12054 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12055 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12056 }
12057
12058 @Test
12059 public void testConstructor102() throws WrongSizeException {
12060 final Collection<StandardDeviationBodyKinematics> measurements =
12061 Collections.emptyList();
12062
12063 final Matrix ba = generateBa();
12064 final double biasX = ba.getElementAtIndex(0);
12065 final double biasY = ba.getElementAtIndex(1);
12066 final double biasZ = ba.getElementAtIndex(2);
12067
12068 final Acceleration bx = new Acceleration(biasX,
12069 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12070 final Acceleration by = new Acceleration(biasY,
12071 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12072 final Acceleration bz = new Acceleration(biasZ,
12073 AccelerationUnit.METERS_PER_SQUARED_SECOND);
12074
12075 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12076 final double latitude = Math.toRadians(
12077 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12078 final double longitude = Math.toRadians(
12079 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12080 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12081 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12082 final NEDVelocity nedVelocity = new NEDVelocity();
12083 final ECEFPosition ecefPosition = new ECEFPosition();
12084 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12085 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12086 ecefPosition, ecefVelocity);
12087
12088 final KnownPositionAccelerometerCalibrator calibrator =
12089 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12090 true, bx, by, bz, this);
12091
12092
12093 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12094 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12095 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12096 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12097 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12098 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12099 final Acceleration bx2 = new Acceleration(0.0,
12100 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12101 calibrator.getInitialBiasXAsAcceleration(bx2);
12102 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12103 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12104 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12105 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12106 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12107 final Acceleration by2 = new Acceleration(0.0,
12108 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12109 calibrator.getInitialBiasYAsAcceleration(by2);
12110 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12111 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12112 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12113 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12114 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12115 final Acceleration bz2 = new Acceleration(0.0,
12116 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12117 calibrator.getInitialBiasZAsAcceleration(bz2);
12118 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12119 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12120 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
12121 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
12122 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
12123 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12124 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12125 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12126 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12127 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12128 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12129 final double[] bias1 = calibrator.getInitialBias();
12130 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12131 final double[] bias2 = new double[3];
12132 calibrator.getInitialBias(bias2);
12133 assertArrayEquals(bias1, bias2, 0.0);
12134 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12135 assertEquals(b1, ba);
12136 final Matrix b2 = new Matrix(3, 1);
12137 calibrator.getInitialBiasAsMatrix(b2);
12138 assertEquals(b1, b2);
12139 final Matrix ma1 = calibrator.getInitialMa();
12140 assertEquals(ma1, new Matrix(3, 3));
12141 final Matrix ma2 = new Matrix(3, 3);
12142 calibrator.getInitialMa(ma2);
12143 assertEquals(ma1, ma2);
12144 assertSame(calibrator.getEcefPosition(), ecefPosition);
12145 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12146 final NEDPosition nedPosition1 = new NEDPosition();
12147 assertTrue(calibrator.getNedPosition(nedPosition1));
12148 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12149 assertSame(calibrator.getMeasurements(), measurements);
12150 assertTrue(calibrator.isCommonAxisUsed());
12151 assertSame(calibrator.getListener(), this);
12152 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12153 assertFalse(calibrator.isReady());
12154 assertFalse(calibrator.isRunning());
12155 assertNull(calibrator.getEstimatedBiases());
12156 assertFalse(calibrator.getEstimatedBiases(null));
12157 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12158 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12159 assertNull(calibrator.getEstimatedBiasFx());
12160 assertNull(calibrator.getEstimatedBiasFy());
12161 assertNull(calibrator.getEstimatedBiasFz());
12162 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12163 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12164 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12165 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12166 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12167 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12168 assertNull(calibrator.getEstimatedMa());
12169 assertNull(calibrator.getEstimatedSx());
12170 assertNull(calibrator.getEstimatedSy());
12171 assertNull(calibrator.getEstimatedSz());
12172 assertNull(calibrator.getEstimatedMxy());
12173 assertNull(calibrator.getEstimatedMxz());
12174 assertNull(calibrator.getEstimatedMyx());
12175 assertNull(calibrator.getEstimatedMyz());
12176 assertNull(calibrator.getEstimatedMzx());
12177 assertNull(calibrator.getEstimatedMzy());
12178 assertNull(calibrator.getEstimatedCovariance());
12179 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12180 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12181 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12182 assertNotNull(calibrator.getGroundTruthGravityNorm());
12183 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12184 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12185 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12186 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12187 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12188 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12189 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12190 }
12191
12192 @Test
12193 public void testConstructor103() throws WrongSizeException {
12194 final Matrix ba = generateBa();
12195 final double biasX = ba.getElementAtIndex(0);
12196 final double biasY = ba.getElementAtIndex(1);
12197 final double biasZ = ba.getElementAtIndex(2);
12198
12199 final Matrix ma = generateMaCommonAxis();
12200 final double sx = ma.getElementAt(0, 0);
12201 final double sy = ma.getElementAt(1, 1);
12202 final double sz = ma.getElementAt(2, 2);
12203
12204 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12205 final double latitude = Math.toRadians(
12206 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12207 final double longitude = Math.toRadians(
12208 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12209 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12210 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12211 final NEDVelocity nedVelocity = new NEDVelocity();
12212 final ECEFPosition ecefPosition = new ECEFPosition();
12213 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12214 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12215 ecefPosition, ecefVelocity);
12216
12217 final KnownPositionAccelerometerCalibrator calibrator =
12218 new KnownPositionAccelerometerCalibrator(ecefPosition,
12219 biasX, biasY, biasZ, sx, sy, sz);
12220
12221
12222 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12223 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12224 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12225 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12226 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12227 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12228 final Acceleration bx2 = new Acceleration(0.0,
12229 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12230 calibrator.getInitialBiasXAsAcceleration(bx2);
12231 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12232 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12233 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12234 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12235 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12236 final Acceleration by2 = new Acceleration(0.0,
12237 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12238 calibrator.getInitialBiasYAsAcceleration(by2);
12239 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12240 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12241 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12242 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12243 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12244 final Acceleration bz2 = new Acceleration(0.0,
12245 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12246 calibrator.getInitialBiasZAsAcceleration(bz2);
12247 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12248 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12249 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12250 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12251 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12252 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12253 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12254 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12255 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12256 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12257 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12258 final double[] bias1 = calibrator.getInitialBias();
12259 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12260 final double[] bias2 = new double[3];
12261 calibrator.getInitialBias(bias2);
12262 assertArrayEquals(bias1, bias2, 0.0);
12263 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12264 assertEquals(b1, ba);
12265 final Matrix b2 = new Matrix(3, 1);
12266 calibrator.getInitialBiasAsMatrix(b2);
12267 assertEquals(b1, b2);
12268 final Matrix ma1 = calibrator.getInitialMa();
12269 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12270 final Matrix ma2 = new Matrix(3, 3);
12271 calibrator.getInitialMa(ma2);
12272 assertEquals(ma1, ma2);
12273 assertSame(calibrator.getEcefPosition(), ecefPosition);
12274 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12275 final NEDPosition nedPosition1 = new NEDPosition();
12276 assertTrue(calibrator.getNedPosition(nedPosition1));
12277 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12278 assertNull(calibrator.getMeasurements());
12279 assertFalse(calibrator.isCommonAxisUsed());
12280 assertNull(calibrator.getListener());
12281 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12282 assertFalse(calibrator.isReady());
12283 assertFalse(calibrator.isRunning());
12284 assertNull(calibrator.getEstimatedBiases());
12285 assertFalse(calibrator.getEstimatedBiases(null));
12286 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12287 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12288 assertNull(calibrator.getEstimatedBiasFx());
12289 assertNull(calibrator.getEstimatedBiasFy());
12290 assertNull(calibrator.getEstimatedBiasFz());
12291 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12292 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12293 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12294 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12295 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12296 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12297 assertNull(calibrator.getEstimatedMa());
12298 assertNull(calibrator.getEstimatedSx());
12299 assertNull(calibrator.getEstimatedSy());
12300 assertNull(calibrator.getEstimatedSz());
12301 assertNull(calibrator.getEstimatedMxy());
12302 assertNull(calibrator.getEstimatedMxz());
12303 assertNull(calibrator.getEstimatedMyx());
12304 assertNull(calibrator.getEstimatedMyz());
12305 assertNull(calibrator.getEstimatedMzx());
12306 assertNull(calibrator.getEstimatedMzy());
12307 assertNull(calibrator.getEstimatedCovariance());
12308 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12309 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12310 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12311 assertNotNull(calibrator.getGroundTruthGravityNorm());
12312 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12313 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12314 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12315 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12316 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12317 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12318 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12319 }
12320
12321 @Test
12322 public void testConstructor104() throws WrongSizeException {
12323 final Collection<StandardDeviationBodyKinematics> measurements =
12324 Collections.emptyList();
12325
12326 final Matrix ba = generateBa();
12327 final double biasX = ba.getElementAtIndex(0);
12328 final double biasY = ba.getElementAtIndex(1);
12329 final double biasZ = ba.getElementAtIndex(2);
12330
12331 final Matrix ma = generateMaCommonAxis();
12332 final double sx = ma.getElementAt(0, 0);
12333 final double sy = ma.getElementAt(1, 1);
12334 final double sz = ma.getElementAt(2, 2);
12335
12336 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12337 final double latitude = Math.toRadians(
12338 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12339 final double longitude = Math.toRadians(
12340 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12341 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12342 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12343 final NEDVelocity nedVelocity = new NEDVelocity();
12344 final ECEFPosition ecefPosition = new ECEFPosition();
12345 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12346 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12347 ecefPosition, ecefVelocity);
12348
12349 final KnownPositionAccelerometerCalibrator calibrator =
12350 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12351 biasX, biasY, biasZ, sx, sy, sz);
12352
12353
12354 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12355 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12356 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12357 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12358 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12359 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12360 final Acceleration bx2 = new Acceleration(0.0,
12361 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12362 calibrator.getInitialBiasXAsAcceleration(bx2);
12363 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12364 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12365 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12366 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12367 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12368 final Acceleration by2 = new Acceleration(0.0,
12369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12370 calibrator.getInitialBiasYAsAcceleration(by2);
12371 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12372 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12373 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12374 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12375 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12376 final Acceleration bz2 = new Acceleration(0.0,
12377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12378 calibrator.getInitialBiasZAsAcceleration(bz2);
12379 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12380 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12381 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12382 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12383 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12384 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12385 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12386 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12387 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12388 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12389 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12390 final double[] bias1 = calibrator.getInitialBias();
12391 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12392 final double[] bias2 = new double[3];
12393 calibrator.getInitialBias(bias2);
12394 assertArrayEquals(bias1, bias2, 0.0);
12395 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12396 assertEquals(b1, ba);
12397 final Matrix b2 = new Matrix(3, 1);
12398 calibrator.getInitialBiasAsMatrix(b2);
12399 assertEquals(b1, b2);
12400 final Matrix ma1 = calibrator.getInitialMa();
12401 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12402 final Matrix ma2 = new Matrix(3, 3);
12403 calibrator.getInitialMa(ma2);
12404 assertEquals(ma1, ma2);
12405 assertSame(calibrator.getEcefPosition(), ecefPosition);
12406 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12407 final NEDPosition nedPosition1 = new NEDPosition();
12408 assertTrue(calibrator.getNedPosition(nedPosition1));
12409 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12410 assertSame(calibrator.getMeasurements(), measurements);
12411 assertFalse(calibrator.isCommonAxisUsed());
12412 assertNull(calibrator.getListener());
12413 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12414 assertFalse(calibrator.isReady());
12415 assertFalse(calibrator.isRunning());
12416 assertNull(calibrator.getEstimatedBiases());
12417 assertFalse(calibrator.getEstimatedBiases(null));
12418 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12419 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12420 assertNull(calibrator.getEstimatedBiasFx());
12421 assertNull(calibrator.getEstimatedBiasFy());
12422 assertNull(calibrator.getEstimatedBiasFz());
12423 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12424 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12425 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12426 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12427 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12428 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12429 assertNull(calibrator.getEstimatedMa());
12430 assertNull(calibrator.getEstimatedSx());
12431 assertNull(calibrator.getEstimatedSy());
12432 assertNull(calibrator.getEstimatedSz());
12433 assertNull(calibrator.getEstimatedMxy());
12434 assertNull(calibrator.getEstimatedMxz());
12435 assertNull(calibrator.getEstimatedMyx());
12436 assertNull(calibrator.getEstimatedMyz());
12437 assertNull(calibrator.getEstimatedMzx());
12438 assertNull(calibrator.getEstimatedMzy());
12439 assertNull(calibrator.getEstimatedCovariance());
12440 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12441 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12442 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12443 assertNotNull(calibrator.getGroundTruthGravityNorm());
12444 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12445 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12446 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12447 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12448 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12449 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12450 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12451 }
12452
12453 @Test
12454 public void testConstructor105() throws WrongSizeException {
12455 final Collection<StandardDeviationBodyKinematics> measurements =
12456 Collections.emptyList();
12457
12458 final Matrix ba = generateBa();
12459 final double biasX = ba.getElementAtIndex(0);
12460 final double biasY = ba.getElementAtIndex(1);
12461 final double biasZ = ba.getElementAtIndex(2);
12462
12463 final Matrix ma = generateMaCommonAxis();
12464 final double sx = ma.getElementAt(0, 0);
12465 final double sy = ma.getElementAt(1, 1);
12466 final double sz = ma.getElementAt(2, 2);
12467
12468 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12469 final double latitude = Math.toRadians(
12470 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12471 final double longitude = Math.toRadians(
12472 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12473 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12474 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12475 final NEDVelocity nedVelocity = new NEDVelocity();
12476 final ECEFPosition ecefPosition = new ECEFPosition();
12477 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12478 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12479 ecefPosition, ecefVelocity);
12480
12481 final KnownPositionAccelerometerCalibrator calibrator =
12482 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12483 biasX, biasY, biasZ, sx, sy, sz, this);
12484
12485
12486 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12487 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12488 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12489 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12490 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12491 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12492 final Acceleration bx2 = new Acceleration(0.0,
12493 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12494 calibrator.getInitialBiasXAsAcceleration(bx2);
12495 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12496 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12497 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12498 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12499 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12500 final Acceleration by2 = new Acceleration(0.0,
12501 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12502 calibrator.getInitialBiasYAsAcceleration(by2);
12503 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12504 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12505 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12506 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12507 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12508 final Acceleration bz2 = new Acceleration(0.0,
12509 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12510 calibrator.getInitialBiasZAsAcceleration(bz2);
12511 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12512 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12513 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12514 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12515 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12516 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12517 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12518 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12519 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12520 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12521 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12522 final double[] bias1 = calibrator.getInitialBias();
12523 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12524 final double[] bias2 = new double[3];
12525 calibrator.getInitialBias(bias2);
12526 assertArrayEquals(bias1, bias2, 0.0);
12527 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12528 assertEquals(b1, ba);
12529 final Matrix b2 = new Matrix(3, 1);
12530 calibrator.getInitialBiasAsMatrix(b2);
12531 assertEquals(b1, b2);
12532 final Matrix ma1 = calibrator.getInitialMa();
12533 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12534 final Matrix ma2 = new Matrix(3, 3);
12535 calibrator.getInitialMa(ma2);
12536 assertEquals(ma1, ma2);
12537 assertSame(calibrator.getEcefPosition(), ecefPosition);
12538 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12539 final NEDPosition nedPosition1 = new NEDPosition();
12540 assertTrue(calibrator.getNedPosition(nedPosition1));
12541 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12542 assertSame(calibrator.getMeasurements(), measurements);
12543 assertFalse(calibrator.isCommonAxisUsed());
12544 assertSame(calibrator.getListener(), this);
12545 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
12546 assertFalse(calibrator.isReady());
12547 assertFalse(calibrator.isRunning());
12548 assertNull(calibrator.getEstimatedBiases());
12549 assertFalse(calibrator.getEstimatedBiases(null));
12550 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12551 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12552 assertNull(calibrator.getEstimatedBiasFx());
12553 assertNull(calibrator.getEstimatedBiasFy());
12554 assertNull(calibrator.getEstimatedBiasFz());
12555 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12556 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12557 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12558 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12559 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12560 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12561 assertNull(calibrator.getEstimatedMa());
12562 assertNull(calibrator.getEstimatedSx());
12563 assertNull(calibrator.getEstimatedSy());
12564 assertNull(calibrator.getEstimatedSz());
12565 assertNull(calibrator.getEstimatedMxy());
12566 assertNull(calibrator.getEstimatedMxz());
12567 assertNull(calibrator.getEstimatedMyx());
12568 assertNull(calibrator.getEstimatedMyz());
12569 assertNull(calibrator.getEstimatedMzx());
12570 assertNull(calibrator.getEstimatedMzy());
12571 assertNull(calibrator.getEstimatedCovariance());
12572 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12573 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12574 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12575 assertNotNull(calibrator.getGroundTruthGravityNorm());
12576 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12577 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12578 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12579 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12580 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12581 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12582 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12583 }
12584
12585 @Test
12586 public void testConstructor106() throws WrongSizeException {
12587 final Matrix ba = generateBa();
12588 final double biasX = ba.getElementAtIndex(0);
12589 final double biasY = ba.getElementAtIndex(1);
12590 final double biasZ = ba.getElementAtIndex(2);
12591
12592 final Matrix ma = generateMaCommonAxis();
12593 final double sx = ma.getElementAt(0, 0);
12594 final double sy = ma.getElementAt(1, 1);
12595 final double sz = ma.getElementAt(2, 2);
12596
12597 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12598 final double latitude = Math.toRadians(
12599 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12600 final double longitude = Math.toRadians(
12601 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12602 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12603 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12604 final NEDVelocity nedVelocity = new NEDVelocity();
12605 final ECEFPosition ecefPosition = new ECEFPosition();
12606 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12607 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12608 ecefPosition, ecefVelocity);
12609
12610 final KnownPositionAccelerometerCalibrator calibrator =
12611 new KnownPositionAccelerometerCalibrator(ecefPosition,
12612 true, biasX, biasY, biasZ, sx, sy, sz);
12613
12614
12615 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12616 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12617 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12618 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12619 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12620 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12621 final Acceleration bx2 = new Acceleration(0.0,
12622 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12623 calibrator.getInitialBiasXAsAcceleration(bx2);
12624 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12625 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12626 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12627 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12628 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12629 final Acceleration by2 = new Acceleration(0.0,
12630 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12631 calibrator.getInitialBiasYAsAcceleration(by2);
12632 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12633 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12634 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12635 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12636 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12637 final Acceleration bz2 = new Acceleration(0.0,
12638 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12639 calibrator.getInitialBiasZAsAcceleration(bz2);
12640 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12641 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12642 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12643 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12644 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12645 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12646 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12647 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12648 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12649 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12650 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12651 final double[] bias1 = calibrator.getInitialBias();
12652 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12653 final double[] bias2 = new double[3];
12654 calibrator.getInitialBias(bias2);
12655 assertArrayEquals(bias1, bias2, 0.0);
12656 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12657 assertEquals(b1, ba);
12658 final Matrix b2 = new Matrix(3, 1);
12659 calibrator.getInitialBiasAsMatrix(b2);
12660 assertEquals(b1, b2);
12661 final Matrix ma1 = calibrator.getInitialMa();
12662 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12663 final Matrix ma2 = new Matrix(3, 3);
12664 calibrator.getInitialMa(ma2);
12665 assertEquals(ma1, ma2);
12666 assertSame(calibrator.getEcefPosition(), ecefPosition);
12667 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12668 final NEDPosition nedPosition1 = new NEDPosition();
12669 assertTrue(calibrator.getNedPosition(nedPosition1));
12670 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12671 assertNull(calibrator.getMeasurements());
12672 assertTrue(calibrator.isCommonAxisUsed());
12673 assertNull(calibrator.getListener());
12674 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12675 assertFalse(calibrator.isReady());
12676 assertFalse(calibrator.isRunning());
12677 assertNull(calibrator.getEstimatedBiases());
12678 assertFalse(calibrator.getEstimatedBiases(null));
12679 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12680 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12681 assertNull(calibrator.getEstimatedBiasFx());
12682 assertNull(calibrator.getEstimatedBiasFy());
12683 assertNull(calibrator.getEstimatedBiasFz());
12684 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12685 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12686 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12687 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12688 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12689 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12690 assertNull(calibrator.getEstimatedMa());
12691 assertNull(calibrator.getEstimatedSx());
12692 assertNull(calibrator.getEstimatedSy());
12693 assertNull(calibrator.getEstimatedSz());
12694 assertNull(calibrator.getEstimatedMxy());
12695 assertNull(calibrator.getEstimatedMxz());
12696 assertNull(calibrator.getEstimatedMyx());
12697 assertNull(calibrator.getEstimatedMyz());
12698 assertNull(calibrator.getEstimatedMzx());
12699 assertNull(calibrator.getEstimatedMzy());
12700 assertNull(calibrator.getEstimatedCovariance());
12701 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12702 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12703 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12704 assertNotNull(calibrator.getGroundTruthGravityNorm());
12705 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12706 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12707 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12708 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12709 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12710 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12711 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12712 }
12713
12714 @Test
12715 public void testConstructor107() throws WrongSizeException {
12716 final Matrix ba = generateBa();
12717 final double biasX = ba.getElementAtIndex(0);
12718 final double biasY = ba.getElementAtIndex(1);
12719 final double biasZ = ba.getElementAtIndex(2);
12720
12721 final Matrix ma = generateMaCommonAxis();
12722 final double sx = ma.getElementAt(0, 0);
12723 final double sy = ma.getElementAt(1, 1);
12724 final double sz = ma.getElementAt(2, 2);
12725
12726 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12727 final double latitude = Math.toRadians(
12728 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12729 final double longitude = Math.toRadians(
12730 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12731 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12732 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12733 final NEDVelocity nedVelocity = new NEDVelocity();
12734 final ECEFPosition ecefPosition = new ECEFPosition();
12735 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12736 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12737 ecefPosition, ecefVelocity);
12738
12739 final KnownPositionAccelerometerCalibrator calibrator =
12740 new KnownPositionAccelerometerCalibrator(ecefPosition,
12741 true, biasX, biasY, biasZ, sx, sy, sz,
12742 this);
12743
12744
12745 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12746 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12747 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12748 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12749 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12750 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12751 final Acceleration bx2 = new Acceleration(0.0,
12752 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12753 calibrator.getInitialBiasXAsAcceleration(bx2);
12754 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12755 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12756 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12757 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12758 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12759 final Acceleration by2 = new Acceleration(0.0,
12760 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12761 calibrator.getInitialBiasYAsAcceleration(by2);
12762 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12763 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12764 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12765 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12766 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12767 final Acceleration bz2 = new Acceleration(0.0,
12768 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12769 calibrator.getInitialBiasZAsAcceleration(bz2);
12770 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12771 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12772 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12773 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12774 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12775 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12776 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12777 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12778 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12779 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12780 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12781 final double[] bias1 = calibrator.getInitialBias();
12782 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12783 final double[] bias2 = new double[3];
12784 calibrator.getInitialBias(bias2);
12785 assertArrayEquals(bias1, bias2, 0.0);
12786 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12787 assertEquals(b1, ba);
12788 final Matrix b2 = new Matrix(3, 1);
12789 calibrator.getInitialBiasAsMatrix(b2);
12790 assertEquals(b1, b2);
12791 final Matrix ma1 = calibrator.getInitialMa();
12792 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12793 final Matrix ma2 = new Matrix(3, 3);
12794 calibrator.getInitialMa(ma2);
12795 assertEquals(ma1, ma2);
12796 assertSame(calibrator.getEcefPosition(), ecefPosition);
12797 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12798 final NEDPosition nedPosition1 = new NEDPosition();
12799 assertTrue(calibrator.getNedPosition(nedPosition1));
12800 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12801 assertNull(calibrator.getMeasurements());
12802 assertTrue(calibrator.isCommonAxisUsed());
12803 assertSame(calibrator.getListener(), this);
12804 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12805 assertFalse(calibrator.isReady());
12806 assertFalse(calibrator.isRunning());
12807 assertNull(calibrator.getEstimatedBiases());
12808 assertFalse(calibrator.getEstimatedBiases(null));
12809 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12810 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12811 assertNull(calibrator.getEstimatedBiasFx());
12812 assertNull(calibrator.getEstimatedBiasFy());
12813 assertNull(calibrator.getEstimatedBiasFz());
12814 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12815 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12816 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12817 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12818 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12819 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12820 assertNull(calibrator.getEstimatedMa());
12821 assertNull(calibrator.getEstimatedSx());
12822 assertNull(calibrator.getEstimatedSy());
12823 assertNull(calibrator.getEstimatedSz());
12824 assertNull(calibrator.getEstimatedMxy());
12825 assertNull(calibrator.getEstimatedMxz());
12826 assertNull(calibrator.getEstimatedMyx());
12827 assertNull(calibrator.getEstimatedMyz());
12828 assertNull(calibrator.getEstimatedMzx());
12829 assertNull(calibrator.getEstimatedMzy());
12830 assertNull(calibrator.getEstimatedCovariance());
12831 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12832 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12833 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12834 assertNotNull(calibrator.getGroundTruthGravityNorm());
12835 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12836 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12837 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12838 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12839 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12840 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12841 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12842 }
12843
12844 @Test
12845 public void testConstructor108() throws WrongSizeException {
12846 final Collection<StandardDeviationBodyKinematics> measurements =
12847 Collections.emptyList();
12848
12849 final Matrix ba = generateBa();
12850 final double biasX = ba.getElementAtIndex(0);
12851 final double biasY = ba.getElementAtIndex(1);
12852 final double biasZ = ba.getElementAtIndex(2);
12853
12854 final Matrix ma = generateMaCommonAxis();
12855 final double sx = ma.getElementAt(0, 0);
12856 final double sy = ma.getElementAt(1, 1);
12857 final double sz = ma.getElementAt(2, 2);
12858
12859 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12860 final double latitude = Math.toRadians(
12861 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12862 final double longitude = Math.toRadians(
12863 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12864 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12865 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12866 final NEDVelocity nedVelocity = new NEDVelocity();
12867 final ECEFPosition ecefPosition = new ECEFPosition();
12868 final ECEFVelocity ecefVelocity = new ECEFVelocity();
12869 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
12870 ecefPosition, ecefVelocity);
12871
12872 final KnownPositionAccelerometerCalibrator calibrator =
12873 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
12874 true, biasX, biasY, biasZ, sx, sy, sz);
12875
12876
12877 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
12878 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
12879 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
12880 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
12881 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
12882 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12883 final Acceleration bx2 = new Acceleration(0.0,
12884 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12885 calibrator.getInitialBiasXAsAcceleration(bx2);
12886 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
12887 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12888 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
12889 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
12890 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12891 final Acceleration by2 = new Acceleration(0.0,
12892 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12893 calibrator.getInitialBiasYAsAcceleration(by2);
12894 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
12895 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12896 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
12897 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
12898 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12899 final Acceleration bz2 = new Acceleration(0.0,
12900 AccelerationUnit.FEET_PER_SQUARED_SECOND);
12901 calibrator.getInitialBiasZAsAcceleration(bz2);
12902 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
12903 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
12904 assertEquals(calibrator.getInitialSx(), sx, 0.0);
12905 assertEquals(calibrator.getInitialSy(), sy, 0.0);
12906 assertEquals(calibrator.getInitialSz(), sz, 0.0);
12907 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
12908 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
12909 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
12910 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
12911 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
12912 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
12913 final double[] bias1 = calibrator.getInitialBias();
12914 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
12915 final double[] bias2 = new double[3];
12916 calibrator.getInitialBias(bias2);
12917 assertArrayEquals(bias1, bias2, 0.0);
12918 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
12919 assertEquals(b1, ba);
12920 final Matrix b2 = new Matrix(3, 1);
12921 calibrator.getInitialBiasAsMatrix(b2);
12922 assertEquals(b1, b2);
12923 final Matrix ma1 = calibrator.getInitialMa();
12924 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
12925 final Matrix ma2 = new Matrix(3, 3);
12926 calibrator.getInitialMa(ma2);
12927 assertEquals(ma1, ma2);
12928 assertSame(calibrator.getEcefPosition(), ecefPosition);
12929 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
12930 final NEDPosition nedPosition1 = new NEDPosition();
12931 assertTrue(calibrator.getNedPosition(nedPosition1));
12932 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
12933 assertSame(calibrator.getMeasurements(), measurements);
12934 assertTrue(calibrator.isCommonAxisUsed());
12935 assertNull(calibrator.getListener());
12936 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
12937 assertFalse(calibrator.isReady());
12938 assertFalse(calibrator.isRunning());
12939 assertNull(calibrator.getEstimatedBiases());
12940 assertFalse(calibrator.getEstimatedBiases(null));
12941 assertNull(calibrator.getEstimatedBiasesAsMatrix());
12942 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
12943 assertNull(calibrator.getEstimatedBiasFx());
12944 assertNull(calibrator.getEstimatedBiasFy());
12945 assertNull(calibrator.getEstimatedBiasFz());
12946 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
12947 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
12948 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
12949 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
12950 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
12951 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
12952 assertNull(calibrator.getEstimatedMa());
12953 assertNull(calibrator.getEstimatedSx());
12954 assertNull(calibrator.getEstimatedSy());
12955 assertNull(calibrator.getEstimatedSz());
12956 assertNull(calibrator.getEstimatedMxy());
12957 assertNull(calibrator.getEstimatedMxz());
12958 assertNull(calibrator.getEstimatedMyx());
12959 assertNull(calibrator.getEstimatedMyz());
12960 assertNull(calibrator.getEstimatedMzx());
12961 assertNull(calibrator.getEstimatedMzy());
12962 assertNull(calibrator.getEstimatedCovariance());
12963 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
12964 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
12965 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
12966 assertNotNull(calibrator.getGroundTruthGravityNorm());
12967 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
12968 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
12969 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
12970 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
12971 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
12972 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
12973 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
12974 }
12975
12976 @Test
12977 public void testConstructor109() throws WrongSizeException {
12978 final Collection<StandardDeviationBodyKinematics> measurements =
12979 Collections.emptyList();
12980
12981 final Matrix ba = generateBa();
12982 final double biasX = ba.getElementAtIndex(0);
12983 final double biasY = ba.getElementAtIndex(1);
12984 final double biasZ = ba.getElementAtIndex(2);
12985
12986 final Matrix ma = generateMaCommonAxis();
12987 final double sx = ma.getElementAt(0, 0);
12988 final double sy = ma.getElementAt(1, 1);
12989 final double sz = ma.getElementAt(2, 2);
12990
12991 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
12992 final double latitude = Math.toRadians(
12993 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
12994 final double longitude = Math.toRadians(
12995 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
12996 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
12997 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
12998 final NEDVelocity nedVelocity = new NEDVelocity();
12999 final ECEFPosition ecefPosition = new ECEFPosition();
13000 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13001 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13002 ecefPosition, ecefVelocity);
13003
13004 final KnownPositionAccelerometerCalibrator calibrator =
13005 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13006 true, biasX, biasY, biasZ, sx, sy, sz,
13007 this);
13008
13009
13010 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13011 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13012 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13013 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13014 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13015 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13016 final Acceleration bx2 = new Acceleration(0.0,
13017 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13018 calibrator.getInitialBiasXAsAcceleration(bx2);
13019 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13020 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13021 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13022 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13023 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13024 final Acceleration by2 = new Acceleration(0.0,
13025 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13026 calibrator.getInitialBiasYAsAcceleration(by2);
13027 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13028 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13029 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13030 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13031 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13032 final Acceleration bz2 = new Acceleration(0.0,
13033 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13034 calibrator.getInitialBiasZAsAcceleration(bz2);
13035 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13036 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13037 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13038 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13039 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13040 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13041 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13042 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13043 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13044 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13045 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13046 final double[] bias1 = calibrator.getInitialBias();
13047 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13048 final double[] bias2 = new double[3];
13049 calibrator.getInitialBias(bias2);
13050 assertArrayEquals(bias1, bias2, 0.0);
13051 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13052 assertEquals(b1, ba);
13053 final Matrix b2 = new Matrix(3, 1);
13054 calibrator.getInitialBiasAsMatrix(b2);
13055 assertEquals(b1, b2);
13056 final Matrix ma1 = calibrator.getInitialMa();
13057 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13058 final Matrix ma2 = new Matrix(3, 3);
13059 calibrator.getInitialMa(ma2);
13060 assertEquals(ma1, ma2);
13061 assertSame(calibrator.getEcefPosition(), ecefPosition);
13062 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13063 final NEDPosition nedPosition1 = new NEDPosition();
13064 assertTrue(calibrator.getNedPosition(nedPosition1));
13065 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13066 assertSame(calibrator.getMeasurements(), measurements);
13067 assertTrue(calibrator.isCommonAxisUsed());
13068 assertSame(calibrator.getListener(), this);
13069 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13070 assertFalse(calibrator.isReady());
13071 assertFalse(calibrator.isRunning());
13072 assertNull(calibrator.getEstimatedBiases());
13073 assertFalse(calibrator.getEstimatedBiases(null));
13074 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13075 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13076 assertNull(calibrator.getEstimatedBiasFx());
13077 assertNull(calibrator.getEstimatedBiasFy());
13078 assertNull(calibrator.getEstimatedBiasFz());
13079 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13080 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13081 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13082 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13083 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13084 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13085 assertNull(calibrator.getEstimatedMa());
13086 assertNull(calibrator.getEstimatedSx());
13087 assertNull(calibrator.getEstimatedSy());
13088 assertNull(calibrator.getEstimatedSz());
13089 assertNull(calibrator.getEstimatedMxy());
13090 assertNull(calibrator.getEstimatedMxz());
13091 assertNull(calibrator.getEstimatedMyx());
13092 assertNull(calibrator.getEstimatedMyz());
13093 assertNull(calibrator.getEstimatedMzx());
13094 assertNull(calibrator.getEstimatedMzy());
13095 assertNull(calibrator.getEstimatedCovariance());
13096 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13097 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13098 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13099 assertNotNull(calibrator.getGroundTruthGravityNorm());
13100 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13101 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13102 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13103 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13104 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13105 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13106 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13107 }
13108
13109 @Test
13110 public void testConstructor110() throws WrongSizeException {
13111 final Matrix ba = generateBa();
13112 final double biasX = ba.getElementAtIndex(0);
13113 final double biasY = ba.getElementAtIndex(1);
13114 final double biasZ = ba.getElementAtIndex(2);
13115
13116 final Acceleration bx = new Acceleration(biasX,
13117 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13118 final Acceleration by = new Acceleration(biasY,
13119 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13120 final Acceleration bz = new Acceleration(biasZ,
13121 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13122
13123 final Matrix ma = generateMaCommonAxis();
13124 final double sx = ma.getElementAt(0, 0);
13125 final double sy = ma.getElementAt(1, 1);
13126 final double sz = ma.getElementAt(2, 2);
13127
13128 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13129 final double latitude = Math.toRadians(
13130 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13131 final double longitude = Math.toRadians(
13132 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13133 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13134 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13135 final NEDVelocity nedVelocity = new NEDVelocity();
13136 final ECEFPosition ecefPosition = new ECEFPosition();
13137 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13138 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13139 ecefPosition, ecefVelocity);
13140
13141 final KnownPositionAccelerometerCalibrator calibrator =
13142 new KnownPositionAccelerometerCalibrator(ecefPosition,
13143 bx, by, bz, sx, sy, sz);
13144
13145
13146 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13147 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13148 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13149 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13150 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13151 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13152 final Acceleration bx2 = new Acceleration(0.0,
13153 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13154 calibrator.getInitialBiasXAsAcceleration(bx2);
13155 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13156 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13157 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13158 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13159 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13160 final Acceleration by2 = new Acceleration(0.0,
13161 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13162 calibrator.getInitialBiasYAsAcceleration(by2);
13163 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13164 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13165 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13166 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13167 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13168 final Acceleration bz2 = new Acceleration(0.0,
13169 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13170 calibrator.getInitialBiasZAsAcceleration(bz2);
13171 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13172 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13173 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13174 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13175 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13176 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13177 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13178 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13179 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13180 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13181 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13182 final double[] bias1 = calibrator.getInitialBias();
13183 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13184 final double[] bias2 = new double[3];
13185 calibrator.getInitialBias(bias2);
13186 assertArrayEquals(bias1, bias2, 0.0);
13187 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13188 assertEquals(b1, ba);
13189 final Matrix b2 = new Matrix(3, 1);
13190 calibrator.getInitialBiasAsMatrix(b2);
13191 assertEquals(b1, b2);
13192 final Matrix ma1 = calibrator.getInitialMa();
13193 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13194 final Matrix ma2 = new Matrix(3, 3);
13195 calibrator.getInitialMa(ma2);
13196 assertEquals(ma1, ma2);
13197 assertSame(calibrator.getEcefPosition(), ecefPosition);
13198 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13199 final NEDPosition nedPosition1 = new NEDPosition();
13200 assertTrue(calibrator.getNedPosition(nedPosition1));
13201 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13202 assertNull(calibrator.getMeasurements());
13203 assertFalse(calibrator.isCommonAxisUsed());
13204 assertNull(calibrator.getListener());
13205 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13206 assertFalse(calibrator.isReady());
13207 assertFalse(calibrator.isRunning());
13208 assertNull(calibrator.getEstimatedBiases());
13209 assertFalse(calibrator.getEstimatedBiases(null));
13210 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13211 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13212 assertNull(calibrator.getEstimatedBiasFx());
13213 assertNull(calibrator.getEstimatedBiasFy());
13214 assertNull(calibrator.getEstimatedBiasFz());
13215 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13216 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13217 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13218 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13219 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13220 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13221 assertNull(calibrator.getEstimatedMa());
13222 assertNull(calibrator.getEstimatedSx());
13223 assertNull(calibrator.getEstimatedSy());
13224 assertNull(calibrator.getEstimatedSz());
13225 assertNull(calibrator.getEstimatedMxy());
13226 assertNull(calibrator.getEstimatedMxz());
13227 assertNull(calibrator.getEstimatedMyx());
13228 assertNull(calibrator.getEstimatedMyz());
13229 assertNull(calibrator.getEstimatedMzx());
13230 assertNull(calibrator.getEstimatedMzy());
13231 assertNull(calibrator.getEstimatedCovariance());
13232 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13233 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13234 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13235 assertNotNull(calibrator.getGroundTruthGravityNorm());
13236 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13237 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13238 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13239 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13240 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13241 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13242 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13243 }
13244
13245 @Test
13246 public void testConstructor111() throws WrongSizeException {
13247 final Matrix ba = generateBa();
13248 final double biasX = ba.getElementAtIndex(0);
13249 final double biasY = ba.getElementAtIndex(1);
13250 final double biasZ = ba.getElementAtIndex(2);
13251
13252 final Acceleration bx = new Acceleration(biasX,
13253 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13254 final Acceleration by = new Acceleration(biasY,
13255 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13256 final Acceleration bz = new Acceleration(biasZ,
13257 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13258
13259 final Matrix ma = generateMaCommonAxis();
13260 final double sx = ma.getElementAt(0, 0);
13261 final double sy = ma.getElementAt(1, 1);
13262 final double sz = ma.getElementAt(2, 2);
13263
13264 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13265 final double latitude = Math.toRadians(
13266 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13267 final double longitude = Math.toRadians(
13268 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13269 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13270 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13271 final NEDVelocity nedVelocity = new NEDVelocity();
13272 final ECEFPosition ecefPosition = new ECEFPosition();
13273 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13274 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13275 ecefPosition, ecefVelocity);
13276
13277 final KnownPositionAccelerometerCalibrator calibrator =
13278 new KnownPositionAccelerometerCalibrator(ecefPosition,
13279 bx, by, bz, sx, sy, sz, this);
13280
13281
13282 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13283 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13284 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13285 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13286 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13287 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13288 final Acceleration bx2 = new Acceleration(0.0,
13289 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13290 calibrator.getInitialBiasXAsAcceleration(bx2);
13291 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13292 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13293 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13294 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13295 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13296 final Acceleration by2 = new Acceleration(0.0,
13297 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13298 calibrator.getInitialBiasYAsAcceleration(by2);
13299 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13300 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13301 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13302 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13303 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13304 final Acceleration bz2 = new Acceleration(0.0,
13305 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13306 calibrator.getInitialBiasZAsAcceleration(bz2);
13307 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13308 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13309 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13310 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13311 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13312 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13313 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13314 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13315 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13316 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13317 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13318 final double[] bias1 = calibrator.getInitialBias();
13319 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13320 final double[] bias2 = new double[3];
13321 calibrator.getInitialBias(bias2);
13322 assertArrayEquals(bias1, bias2, 0.0);
13323 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13324 assertEquals(b1, ba);
13325 final Matrix b2 = new Matrix(3, 1);
13326 calibrator.getInitialBiasAsMatrix(b2);
13327 assertEquals(b1, b2);
13328 final Matrix ma1 = calibrator.getInitialMa();
13329 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13330 final Matrix ma2 = new Matrix(3, 3);
13331 calibrator.getInitialMa(ma2);
13332 assertEquals(ma1, ma2);
13333 assertSame(calibrator.getEcefPosition(), ecefPosition);
13334 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13335 final NEDPosition nedPosition1 = new NEDPosition();
13336 assertTrue(calibrator.getNedPosition(nedPosition1));
13337 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13338 assertNull(calibrator.getMeasurements());
13339 assertFalse(calibrator.isCommonAxisUsed());
13340 assertSame(calibrator.getListener(), this);
13341 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13342 assertFalse(calibrator.isReady());
13343 assertFalse(calibrator.isRunning());
13344 assertNull(calibrator.getEstimatedBiases());
13345 assertFalse(calibrator.getEstimatedBiases(null));
13346 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13347 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13348 assertNull(calibrator.getEstimatedBiasFx());
13349 assertNull(calibrator.getEstimatedBiasFy());
13350 assertNull(calibrator.getEstimatedBiasFz());
13351 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13352 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13353 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13354 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13355 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13356 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13357 assertNull(calibrator.getEstimatedMa());
13358 assertNull(calibrator.getEstimatedSx());
13359 assertNull(calibrator.getEstimatedSy());
13360 assertNull(calibrator.getEstimatedSz());
13361 assertNull(calibrator.getEstimatedMxy());
13362 assertNull(calibrator.getEstimatedMxz());
13363 assertNull(calibrator.getEstimatedMyx());
13364 assertNull(calibrator.getEstimatedMyz());
13365 assertNull(calibrator.getEstimatedMzx());
13366 assertNull(calibrator.getEstimatedMzy());
13367 assertNull(calibrator.getEstimatedCovariance());
13368 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13369 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13370 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13371 assertNotNull(calibrator.getGroundTruthGravityNorm());
13372 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13373 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13374 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13375 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13376 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13377 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13378 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13379 }
13380
13381 @Test
13382 public void testConstructor112() throws WrongSizeException {
13383 final Collection<StandardDeviationBodyKinematics> measurements =
13384 Collections.emptyList();
13385
13386 final Matrix ba = generateBa();
13387 final double biasX = ba.getElementAtIndex(0);
13388 final double biasY = ba.getElementAtIndex(1);
13389 final double biasZ = ba.getElementAtIndex(2);
13390
13391 final Acceleration bx = new Acceleration(biasX,
13392 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13393 final Acceleration by = new Acceleration(biasY,
13394 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13395 final Acceleration bz = new Acceleration(biasZ,
13396 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13397
13398 final Matrix ma = generateMaCommonAxis();
13399 final double sx = ma.getElementAt(0, 0);
13400 final double sy = ma.getElementAt(1, 1);
13401 final double sz = ma.getElementAt(2, 2);
13402
13403 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13404 final double latitude = Math.toRadians(
13405 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13406 final double longitude = Math.toRadians(
13407 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13408 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13409 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13410 final NEDVelocity nedVelocity = new NEDVelocity();
13411 final ECEFPosition ecefPosition = new ECEFPosition();
13412 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13413 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13414 ecefPosition, ecefVelocity);
13415
13416 final KnownPositionAccelerometerCalibrator calibrator =
13417 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13418 bx, by, bz, sx, sy, sz);
13419
13420
13421 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13422 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13423 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13424 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13425 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13426 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13427 final Acceleration bx2 = new Acceleration(0.0,
13428 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13429 calibrator.getInitialBiasXAsAcceleration(bx2);
13430 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13431 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13432 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13433 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13434 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13435 final Acceleration by2 = new Acceleration(0.0,
13436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13437 calibrator.getInitialBiasYAsAcceleration(by2);
13438 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13439 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13440 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13441 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13442 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13443 final Acceleration bz2 = new Acceleration(0.0,
13444 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13445 calibrator.getInitialBiasZAsAcceleration(bz2);
13446 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13447 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13448 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13449 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13450 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13451 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13452 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13453 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13454 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13455 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13456 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13457 final double[] bias1 = calibrator.getInitialBias();
13458 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13459 final double[] bias2 = new double[3];
13460 calibrator.getInitialBias(bias2);
13461 assertArrayEquals(bias1, bias2, 0.0);
13462 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13463 assertEquals(b1, ba);
13464 final Matrix b2 = new Matrix(3, 1);
13465 calibrator.getInitialBiasAsMatrix(b2);
13466 assertEquals(b1, b2);
13467 final Matrix ma1 = calibrator.getInitialMa();
13468 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13469 final Matrix ma2 = new Matrix(3, 3);
13470 calibrator.getInitialMa(ma2);
13471 assertEquals(ma1, ma2);
13472 assertSame(calibrator.getEcefPosition(), ecefPosition);
13473 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13474 final NEDPosition nedPosition1 = new NEDPosition();
13475 assertTrue(calibrator.getNedPosition(nedPosition1));
13476 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13477 assertSame(calibrator.getMeasurements(), measurements);
13478 assertFalse(calibrator.isCommonAxisUsed());
13479 assertNull(calibrator.getListener());
13480 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13481 assertFalse(calibrator.isReady());
13482 assertFalse(calibrator.isRunning());
13483 assertNull(calibrator.getEstimatedBiases());
13484 assertFalse(calibrator.getEstimatedBiases(null));
13485 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13486 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13487 assertNull(calibrator.getEstimatedBiasFx());
13488 assertNull(calibrator.getEstimatedBiasFy());
13489 assertNull(calibrator.getEstimatedBiasFz());
13490 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13491 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13492 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13493 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13494 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13495 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13496 assertNull(calibrator.getEstimatedMa());
13497 assertNull(calibrator.getEstimatedSx());
13498 assertNull(calibrator.getEstimatedSy());
13499 assertNull(calibrator.getEstimatedSz());
13500 assertNull(calibrator.getEstimatedMxy());
13501 assertNull(calibrator.getEstimatedMxz());
13502 assertNull(calibrator.getEstimatedMyx());
13503 assertNull(calibrator.getEstimatedMyz());
13504 assertNull(calibrator.getEstimatedMzx());
13505 assertNull(calibrator.getEstimatedMzy());
13506 assertNull(calibrator.getEstimatedCovariance());
13507 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13508 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13509 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13510 assertNotNull(calibrator.getGroundTruthGravityNorm());
13511 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13512 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13513 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13514 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13515 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13516 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13517 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13518 }
13519
13520 @Test
13521 public void testConstructor113() throws WrongSizeException {
13522 final Collection<StandardDeviationBodyKinematics> measurements =
13523 Collections.emptyList();
13524
13525 final Matrix ba = generateBa();
13526 final double biasX = ba.getElementAtIndex(0);
13527 final double biasY = ba.getElementAtIndex(1);
13528 final double biasZ = ba.getElementAtIndex(2);
13529
13530 final Acceleration bx = new Acceleration(biasX,
13531 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13532 final Acceleration by = new Acceleration(biasY,
13533 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13534 final Acceleration bz = new Acceleration(biasZ,
13535 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13536
13537 final Matrix ma = generateMaCommonAxis();
13538 final double sx = ma.getElementAt(0, 0);
13539 final double sy = ma.getElementAt(1, 1);
13540 final double sz = ma.getElementAt(2, 2);
13541
13542 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13543 final double latitude = Math.toRadians(
13544 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13545 final double longitude = Math.toRadians(
13546 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13547 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13548 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13549 final NEDVelocity nedVelocity = new NEDVelocity();
13550 final ECEFPosition ecefPosition = new ECEFPosition();
13551 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13552 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13553 ecefPosition, ecefVelocity);
13554
13555 final KnownPositionAccelerometerCalibrator calibrator =
13556 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13557 bx, by, bz, sx, sy, sz, this);
13558
13559
13560 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13561 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13562 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13563 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13564 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13565 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13566 final Acceleration bx2 = new Acceleration(0.0,
13567 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13568 calibrator.getInitialBiasXAsAcceleration(bx2);
13569 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13570 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13571 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13572 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13573 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13574 final Acceleration by2 = new Acceleration(0.0,
13575 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13576 calibrator.getInitialBiasYAsAcceleration(by2);
13577 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13578 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13579 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13580 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13581 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13582 final Acceleration bz2 = new Acceleration(0.0,
13583 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13584 calibrator.getInitialBiasZAsAcceleration(bz2);
13585 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13586 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13587 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13588 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13589 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13590 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13591 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13592 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13593 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13594 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13595 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13596 final double[] bias1 = calibrator.getInitialBias();
13597 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13598 final double[] bias2 = new double[3];
13599 calibrator.getInitialBias(bias2);
13600 assertArrayEquals(bias1, bias2, 0.0);
13601 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13602 assertEquals(b1, ba);
13603 final Matrix b2 = new Matrix(3, 1);
13604 calibrator.getInitialBiasAsMatrix(b2);
13605 assertEquals(b1, b2);
13606 final Matrix ma1 = calibrator.getInitialMa();
13607 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13608 final Matrix ma2 = new Matrix(3, 3);
13609 calibrator.getInitialMa(ma2);
13610 assertEquals(ma1, ma2);
13611 assertSame(calibrator.getEcefPosition(), ecefPosition);
13612 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13613 final NEDPosition nedPosition1 = new NEDPosition();
13614 assertTrue(calibrator.getNedPosition(nedPosition1));
13615 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13616 assertSame(calibrator.getMeasurements(), measurements);
13617 assertFalse(calibrator.isCommonAxisUsed());
13618 assertSame(calibrator.getListener(), this);
13619 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
13620 assertFalse(calibrator.isReady());
13621 assertFalse(calibrator.isRunning());
13622 assertNull(calibrator.getEstimatedBiases());
13623 assertFalse(calibrator.getEstimatedBiases(null));
13624 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13625 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13626 assertNull(calibrator.getEstimatedBiasFx());
13627 assertNull(calibrator.getEstimatedBiasFy());
13628 assertNull(calibrator.getEstimatedBiasFz());
13629 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13630 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13631 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13632 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13633 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13634 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13635 assertNull(calibrator.getEstimatedMa());
13636 assertNull(calibrator.getEstimatedSx());
13637 assertNull(calibrator.getEstimatedSy());
13638 assertNull(calibrator.getEstimatedSz());
13639 assertNull(calibrator.getEstimatedMxy());
13640 assertNull(calibrator.getEstimatedMxz());
13641 assertNull(calibrator.getEstimatedMyx());
13642 assertNull(calibrator.getEstimatedMyz());
13643 assertNull(calibrator.getEstimatedMzx());
13644 assertNull(calibrator.getEstimatedMzy());
13645 assertNull(calibrator.getEstimatedCovariance());
13646 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13647 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13648 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13649 assertNotNull(calibrator.getGroundTruthGravityNorm());
13650 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13651 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13652 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13653 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13654 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13655 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13656 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13657 }
13658
13659 @Test
13660 public void testConstructor114() throws WrongSizeException {
13661 final Matrix ba = generateBa();
13662 final double biasX = ba.getElementAtIndex(0);
13663 final double biasY = ba.getElementAtIndex(1);
13664 final double biasZ = ba.getElementAtIndex(2);
13665
13666 final Acceleration bx = new Acceleration(biasX,
13667 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13668 final Acceleration by = new Acceleration(biasY,
13669 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13670 final Acceleration bz = new Acceleration(biasZ,
13671 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13672
13673 final Matrix ma = generateMaCommonAxis();
13674 final double sx = ma.getElementAt(0, 0);
13675 final double sy = ma.getElementAt(1, 1);
13676 final double sz = ma.getElementAt(2, 2);
13677
13678 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13679 final double latitude = Math.toRadians(
13680 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13681 final double longitude = Math.toRadians(
13682 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13683 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13684 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13685 final NEDVelocity nedVelocity = new NEDVelocity();
13686 final ECEFPosition ecefPosition = new ECEFPosition();
13687 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13688 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13689 ecefPosition, ecefVelocity);
13690
13691 final KnownPositionAccelerometerCalibrator calibrator =
13692 new KnownPositionAccelerometerCalibrator(ecefPosition,
13693 true, bx, by, bz, sx, sy, sz);
13694
13695
13696 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13697 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13698 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13699 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13700 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13701 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13702 final Acceleration bx2 = new Acceleration(0.0,
13703 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13704 calibrator.getInitialBiasXAsAcceleration(bx2);
13705 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13706 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13707 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13708 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13709 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13710 final Acceleration by2 = new Acceleration(0.0,
13711 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13712 calibrator.getInitialBiasYAsAcceleration(by2);
13713 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13714 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13715 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13716 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13717 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13718 final Acceleration bz2 = new Acceleration(0.0,
13719 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13720 calibrator.getInitialBiasZAsAcceleration(bz2);
13721 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13722 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13723 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13724 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13725 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13726 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13727 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13728 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13729 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13730 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13731 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13732 final double[] bias1 = calibrator.getInitialBias();
13733 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13734 final double[] bias2 = new double[3];
13735 calibrator.getInitialBias(bias2);
13736 assertArrayEquals(bias1, bias2, 0.0);
13737 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13738 assertEquals(b1, ba);
13739 final Matrix b2 = new Matrix(3, 1);
13740 calibrator.getInitialBiasAsMatrix(b2);
13741 assertEquals(b1, b2);
13742 final Matrix ma1 = calibrator.getInitialMa();
13743 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13744 final Matrix ma2 = new Matrix(3, 3);
13745 calibrator.getInitialMa(ma2);
13746 assertEquals(ma1, ma2);
13747 assertSame(calibrator.getEcefPosition(), ecefPosition);
13748 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13749 final NEDPosition nedPosition1 = new NEDPosition();
13750 assertTrue(calibrator.getNedPosition(nedPosition1));
13751 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13752 assertNull(calibrator.getMeasurements());
13753 assertTrue(calibrator.isCommonAxisUsed());
13754 assertNull(calibrator.getListener());
13755 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13756 assertFalse(calibrator.isReady());
13757 assertFalse(calibrator.isRunning());
13758 assertNull(calibrator.getEstimatedBiases());
13759 assertFalse(calibrator.getEstimatedBiases(null));
13760 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13761 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13762 assertNull(calibrator.getEstimatedBiasFx());
13763 assertNull(calibrator.getEstimatedBiasFy());
13764 assertNull(calibrator.getEstimatedBiasFz());
13765 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13766 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13767 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13768 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13769 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13770 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13771 assertNull(calibrator.getEstimatedMa());
13772 assertNull(calibrator.getEstimatedSx());
13773 assertNull(calibrator.getEstimatedSy());
13774 assertNull(calibrator.getEstimatedSz());
13775 assertNull(calibrator.getEstimatedMxy());
13776 assertNull(calibrator.getEstimatedMxz());
13777 assertNull(calibrator.getEstimatedMyx());
13778 assertNull(calibrator.getEstimatedMyz());
13779 assertNull(calibrator.getEstimatedMzx());
13780 assertNull(calibrator.getEstimatedMzy());
13781 assertNull(calibrator.getEstimatedCovariance());
13782 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13783 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13784 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13785 assertNotNull(calibrator.getGroundTruthGravityNorm());
13786 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13787 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13788 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13789 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13790 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13791 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13792 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13793 }
13794
13795 @Test
13796 public void testConstructor115() throws WrongSizeException {
13797 final Matrix ba = generateBa();
13798 final double biasX = ba.getElementAtIndex(0);
13799 final double biasY = ba.getElementAtIndex(1);
13800 final double biasZ = ba.getElementAtIndex(2);
13801
13802 final Acceleration bx = new Acceleration(biasX,
13803 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13804 final Acceleration by = new Acceleration(biasY,
13805 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13806 final Acceleration bz = new Acceleration(biasZ,
13807 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13808
13809 final Matrix ma = generateMaCommonAxis();
13810 final double sx = ma.getElementAt(0, 0);
13811 final double sy = ma.getElementAt(1, 1);
13812 final double sz = ma.getElementAt(2, 2);
13813
13814 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13815 final double latitude = Math.toRadians(
13816 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13817 final double longitude = Math.toRadians(
13818 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13819 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13820 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13821 final NEDVelocity nedVelocity = new NEDVelocity();
13822 final ECEFPosition ecefPosition = new ECEFPosition();
13823 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13824 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13825 ecefPosition, ecefVelocity);
13826
13827 final KnownPositionAccelerometerCalibrator calibrator =
13828 new KnownPositionAccelerometerCalibrator(ecefPosition,
13829 true, bx, by, bz, sx, sy, sz, this);
13830
13831
13832 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13833 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13834 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13835 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13836 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13837 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13838 final Acceleration bx2 = new Acceleration(0.0,
13839 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13840 calibrator.getInitialBiasXAsAcceleration(bx2);
13841 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13842 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13843 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13844 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13845 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13846 final Acceleration by2 = new Acceleration(0.0,
13847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13848 calibrator.getInitialBiasYAsAcceleration(by2);
13849 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13850 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13851 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13852 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13853 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13854 final Acceleration bz2 = new Acceleration(0.0,
13855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13856 calibrator.getInitialBiasZAsAcceleration(bz2);
13857 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13858 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13859 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13860 assertEquals(calibrator.getInitialSy(), sy, 0.0);
13861 assertEquals(calibrator.getInitialSz(), sz, 0.0);
13862 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
13863 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
13864 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
13865 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
13866 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
13867 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
13868 final double[] bias1 = calibrator.getInitialBias();
13869 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
13870 final double[] bias2 = new double[3];
13871 calibrator.getInitialBias(bias2);
13872 assertArrayEquals(bias1, bias2, 0.0);
13873 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
13874 assertEquals(b1, ba);
13875 final Matrix b2 = new Matrix(3, 1);
13876 calibrator.getInitialBiasAsMatrix(b2);
13877 assertEquals(b1, b2);
13878 final Matrix ma1 = calibrator.getInitialMa();
13879 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
13880 final Matrix ma2 = new Matrix(3, 3);
13881 calibrator.getInitialMa(ma2);
13882 assertEquals(ma1, ma2);
13883 assertSame(calibrator.getEcefPosition(), ecefPosition);
13884 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
13885 final NEDPosition nedPosition1 = new NEDPosition();
13886 assertTrue(calibrator.getNedPosition(nedPosition1));
13887 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
13888 assertNull(calibrator.getMeasurements());
13889 assertTrue(calibrator.isCommonAxisUsed());
13890 assertSame(calibrator.getListener(), this);
13891 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
13892 assertFalse(calibrator.isReady());
13893 assertFalse(calibrator.isRunning());
13894 assertNull(calibrator.getEstimatedBiases());
13895 assertFalse(calibrator.getEstimatedBiases(null));
13896 assertNull(calibrator.getEstimatedBiasesAsMatrix());
13897 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
13898 assertNull(calibrator.getEstimatedBiasFx());
13899 assertNull(calibrator.getEstimatedBiasFy());
13900 assertNull(calibrator.getEstimatedBiasFz());
13901 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
13902 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
13903 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
13904 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
13905 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
13906 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
13907 assertNull(calibrator.getEstimatedMa());
13908 assertNull(calibrator.getEstimatedSx());
13909 assertNull(calibrator.getEstimatedSy());
13910 assertNull(calibrator.getEstimatedSz());
13911 assertNull(calibrator.getEstimatedMxy());
13912 assertNull(calibrator.getEstimatedMxz());
13913 assertNull(calibrator.getEstimatedMyx());
13914 assertNull(calibrator.getEstimatedMyz());
13915 assertNull(calibrator.getEstimatedMzx());
13916 assertNull(calibrator.getEstimatedMzy());
13917 assertNull(calibrator.getEstimatedCovariance());
13918 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
13919 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
13920 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
13921 assertNotNull(calibrator.getGroundTruthGravityNorm());
13922 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
13923 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
13924 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
13925 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
13926 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
13927 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
13928 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
13929 }
13930
13931 @Test
13932 public void testConstructor116() throws WrongSizeException {
13933 final Collection<StandardDeviationBodyKinematics> measurements =
13934 Collections.emptyList();
13935
13936 final Matrix ba = generateBa();
13937 final double biasX = ba.getElementAtIndex(0);
13938 final double biasY = ba.getElementAtIndex(1);
13939 final double biasZ = ba.getElementAtIndex(2);
13940
13941 final Acceleration bx = new Acceleration(biasX,
13942 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13943 final Acceleration by = new Acceleration(biasY,
13944 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13945 final Acceleration bz = new Acceleration(biasZ,
13946 AccelerationUnit.METERS_PER_SQUARED_SECOND);
13947
13948 final Matrix ma = generateMaCommonAxis();
13949 final double sx = ma.getElementAt(0, 0);
13950 final double sy = ma.getElementAt(1, 1);
13951 final double sz = ma.getElementAt(2, 2);
13952
13953 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
13954 final double latitude = Math.toRadians(
13955 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
13956 final double longitude = Math.toRadians(
13957 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
13958 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
13959 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
13960 final NEDVelocity nedVelocity = new NEDVelocity();
13961 final ECEFPosition ecefPosition = new ECEFPosition();
13962 final ECEFVelocity ecefVelocity = new ECEFVelocity();
13963 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
13964 ecefPosition, ecefVelocity);
13965
13966 final KnownPositionAccelerometerCalibrator calibrator =
13967 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
13968 true, bx, by, bz, sx, sy, sz);
13969
13970
13971 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
13972 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
13973 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
13974 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
13975 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
13976 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13977 final Acceleration bx2 = new Acceleration(0.0,
13978 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13979 calibrator.getInitialBiasXAsAcceleration(bx2);
13980 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
13981 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13982 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
13983 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
13984 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13985 final Acceleration by2 = new Acceleration(0.0,
13986 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13987 calibrator.getInitialBiasYAsAcceleration(by2);
13988 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
13989 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13990 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
13991 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
13992 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13993 final Acceleration bz2 = new Acceleration(0.0,
13994 AccelerationUnit.FEET_PER_SQUARED_SECOND);
13995 calibrator.getInitialBiasZAsAcceleration(bz2);
13996 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
13997 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
13998 assertEquals(calibrator.getInitialSx(), sx, 0.0);
13999 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14000 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14001 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14002 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14003 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14004 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14005 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14006 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14007 final double[] bias1 = calibrator.getInitialBias();
14008 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14009 final double[] bias2 = new double[3];
14010 calibrator.getInitialBias(bias2);
14011 assertArrayEquals(bias1, bias2, 0.0);
14012 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14013 assertEquals(b1, ba);
14014 final Matrix b2 = new Matrix(3, 1);
14015 calibrator.getInitialBiasAsMatrix(b2);
14016 assertEquals(b1, b2);
14017 final Matrix ma1 = calibrator.getInitialMa();
14018 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14019 final Matrix ma2 = new Matrix(3, 3);
14020 calibrator.getInitialMa(ma2);
14021 assertEquals(ma1, ma2);
14022 assertSame(calibrator.getEcefPosition(), ecefPosition);
14023 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14024 final NEDPosition nedPosition1 = new NEDPosition();
14025 assertTrue(calibrator.getNedPosition(nedPosition1));
14026 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14027 assertSame(calibrator.getMeasurements(), measurements);
14028 assertTrue(calibrator.isCommonAxisUsed());
14029 assertNull(calibrator.getListener());
14030 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14031 assertFalse(calibrator.isReady());
14032 assertFalse(calibrator.isRunning());
14033 assertNull(calibrator.getEstimatedBiases());
14034 assertFalse(calibrator.getEstimatedBiases(null));
14035 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14036 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14037 assertNull(calibrator.getEstimatedBiasFx());
14038 assertNull(calibrator.getEstimatedBiasFy());
14039 assertNull(calibrator.getEstimatedBiasFz());
14040 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14041 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14042 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14043 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14044 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14045 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14046 assertNull(calibrator.getEstimatedMa());
14047 assertNull(calibrator.getEstimatedSx());
14048 assertNull(calibrator.getEstimatedSy());
14049 assertNull(calibrator.getEstimatedSz());
14050 assertNull(calibrator.getEstimatedMxy());
14051 assertNull(calibrator.getEstimatedMxz());
14052 assertNull(calibrator.getEstimatedMyx());
14053 assertNull(calibrator.getEstimatedMyz());
14054 assertNull(calibrator.getEstimatedMzx());
14055 assertNull(calibrator.getEstimatedMzy());
14056 assertNull(calibrator.getEstimatedCovariance());
14057 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14058 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14059 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14060 assertNotNull(calibrator.getGroundTruthGravityNorm());
14061 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14062 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14063 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14064 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14065 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14066 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14067 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14068 }
14069
14070 @Test
14071 public void testConstructor117() throws WrongSizeException {
14072 final Collection<StandardDeviationBodyKinematics> measurements =
14073 Collections.emptyList();
14074
14075 final Matrix ba = generateBa();
14076 final double biasX = ba.getElementAtIndex(0);
14077 final double biasY = ba.getElementAtIndex(1);
14078 final double biasZ = ba.getElementAtIndex(2);
14079
14080 final Acceleration bx = new Acceleration(biasX,
14081 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14082 final Acceleration by = new Acceleration(biasY,
14083 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14084 final Acceleration bz = new Acceleration(biasZ,
14085 AccelerationUnit.METERS_PER_SQUARED_SECOND);
14086
14087 final Matrix ma = generateMaCommonAxis();
14088 final double sx = ma.getElementAt(0, 0);
14089 final double sy = ma.getElementAt(1, 1);
14090 final double sz = ma.getElementAt(2, 2);
14091
14092 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14093 final double latitude = Math.toRadians(
14094 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14095 final double longitude = Math.toRadians(
14096 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14097 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14098 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14099 final NEDVelocity nedVelocity = new NEDVelocity();
14100 final ECEFPosition ecefPosition = new ECEFPosition();
14101 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14102 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14103 ecefPosition, ecefVelocity);
14104
14105 final KnownPositionAccelerometerCalibrator calibrator =
14106 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14107 true, bx, by, bz, sx, sy, sz, this);
14108
14109
14110 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14111 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14112 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14113 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14114 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14115 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14116 final Acceleration bx2 = new Acceleration(0.0,
14117 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14118 calibrator.getInitialBiasXAsAcceleration(bx2);
14119 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14120 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14121 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14122 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14123 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14124 final Acceleration by2 = new Acceleration(0.0,
14125 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14126 calibrator.getInitialBiasYAsAcceleration(by2);
14127 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14128 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14129 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14130 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14131 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14132 final Acceleration bz2 = new Acceleration(0.0,
14133 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14134 calibrator.getInitialBiasZAsAcceleration(bz2);
14135 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14136 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14137 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14138 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14139 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14140 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
14141 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
14142 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
14143 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
14144 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
14145 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
14146 final double[] bias1 = calibrator.getInitialBias();
14147 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14148 final double[] bias2 = new double[3];
14149 calibrator.getInitialBias(bias2);
14150 assertArrayEquals(bias1, bias2, 0.0);
14151 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14152 assertEquals(b1, ba);
14153 final Matrix b2 = new Matrix(3, 1);
14154 calibrator.getInitialBiasAsMatrix(b2);
14155 assertEquals(b1, b2);
14156 final Matrix ma1 = calibrator.getInitialMa();
14157 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
14158 final Matrix ma2 = new Matrix(3, 3);
14159 calibrator.getInitialMa(ma2);
14160 assertEquals(ma1, ma2);
14161 assertSame(calibrator.getEcefPosition(), ecefPosition);
14162 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14163 final NEDPosition nedPosition1 = new NEDPosition();
14164 assertTrue(calibrator.getNedPosition(nedPosition1));
14165 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14166 assertSame(calibrator.getMeasurements(), measurements);
14167 assertTrue(calibrator.isCommonAxisUsed());
14168 assertSame(calibrator.getListener(), this);
14169 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14170 assertFalse(calibrator.isReady());
14171 assertFalse(calibrator.isRunning());
14172 assertNull(calibrator.getEstimatedBiases());
14173 assertFalse(calibrator.getEstimatedBiases(null));
14174 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14175 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14176 assertNull(calibrator.getEstimatedBiasFx());
14177 assertNull(calibrator.getEstimatedBiasFy());
14178 assertNull(calibrator.getEstimatedBiasFz());
14179 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14180 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14181 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14182 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14183 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14184 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14185 assertNull(calibrator.getEstimatedMa());
14186 assertNull(calibrator.getEstimatedSx());
14187 assertNull(calibrator.getEstimatedSy());
14188 assertNull(calibrator.getEstimatedSz());
14189 assertNull(calibrator.getEstimatedMxy());
14190 assertNull(calibrator.getEstimatedMxz());
14191 assertNull(calibrator.getEstimatedMyx());
14192 assertNull(calibrator.getEstimatedMyz());
14193 assertNull(calibrator.getEstimatedMzx());
14194 assertNull(calibrator.getEstimatedMzy());
14195 assertNull(calibrator.getEstimatedCovariance());
14196 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14197 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14198 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14199 assertNotNull(calibrator.getGroundTruthGravityNorm());
14200 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14201 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14202 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14203 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14204 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14205 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14206 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14207 }
14208
14209 @Test
14210 public void testConstructor118() throws WrongSizeException {
14211 final Matrix ba = generateBa();
14212 final double biasX = ba.getElementAtIndex(0);
14213 final double biasY = ba.getElementAtIndex(1);
14214 final double biasZ = ba.getElementAtIndex(2);
14215
14216 final Matrix ma = generateMaCommonAxis();
14217 final double sx = ma.getElementAt(0, 0);
14218 final double sy = ma.getElementAt(1, 1);
14219 final double sz = ma.getElementAt(2, 2);
14220 final double mxy = ma.getElementAt(0, 1);
14221 final double mxz = ma.getElementAt(0, 2);
14222 final double myx = ma.getElementAt(1, 0);
14223 final double myz = ma.getElementAt(1, 2);
14224 final double mzx = ma.getElementAt(2, 0);
14225 final double mzy = ma.getElementAt(2, 1);
14226
14227 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14228 final double latitude = Math.toRadians(
14229 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14230 final double longitude = Math.toRadians(
14231 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14232 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14233 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14234 final NEDVelocity nedVelocity = new NEDVelocity();
14235 final ECEFPosition ecefPosition = new ECEFPosition();
14236 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14237 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14238 ecefPosition, ecefVelocity);
14239
14240 final KnownPositionAccelerometerCalibrator calibrator =
14241 new KnownPositionAccelerometerCalibrator(ecefPosition,
14242 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14243 myx, myz, mzx, mzy);
14244
14245
14246 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14247 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14248 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14249 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14250 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14251 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14252 final Acceleration bx2 = new Acceleration(0.0,
14253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14254 calibrator.getInitialBiasXAsAcceleration(bx2);
14255 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14256 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14257 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14258 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14259 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14260 final Acceleration by2 = new Acceleration(0.0,
14261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14262 calibrator.getInitialBiasYAsAcceleration(by2);
14263 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14264 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14265 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14266 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14267 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14268 final Acceleration bz2 = new Acceleration(0.0,
14269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14270 calibrator.getInitialBiasZAsAcceleration(bz2);
14271 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14272 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14273 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14274 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14275 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14276 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14277 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14278 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14279 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14280 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14281 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14282 final double[] bias1 = calibrator.getInitialBias();
14283 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14284 final double[] bias2 = new double[3];
14285 calibrator.getInitialBias(bias2);
14286 assertArrayEquals(bias1, bias2, 0.0);
14287 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14288 assertEquals(b1, ba);
14289 final Matrix b2 = new Matrix(3, 1);
14290 calibrator.getInitialBiasAsMatrix(b2);
14291 assertEquals(b1, b2);
14292 final Matrix ma1 = new Matrix(3, 3);
14293 ma1.setSubmatrix(0, 0,
14294 2, 2,
14295 new double[]{sx, myx, mzx,
14296 mxy, sy, mzy,
14297 mxz, myz, sz});
14298 assertEquals(calibrator.getInitialMa(), ma1);
14299 final Matrix ma2 = new Matrix(3, 3);
14300 calibrator.getInitialMa(ma2);
14301 assertEquals(ma1, ma2);
14302 assertSame(calibrator.getEcefPosition(), ecefPosition);
14303 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14304 final NEDPosition nedPosition1 = new NEDPosition();
14305 assertTrue(calibrator.getNedPosition(nedPosition1));
14306 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14307 assertNull(calibrator.getMeasurements());
14308 assertFalse(calibrator.isCommonAxisUsed());
14309 assertNull(calibrator.getListener());
14310 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14311 assertFalse(calibrator.isReady());
14312 assertFalse(calibrator.isRunning());
14313 assertNull(calibrator.getEstimatedBiases());
14314 assertFalse(calibrator.getEstimatedBiases(null));
14315 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14316 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14317 assertNull(calibrator.getEstimatedBiasFx());
14318 assertNull(calibrator.getEstimatedBiasFy());
14319 assertNull(calibrator.getEstimatedBiasFz());
14320 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14321 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14322 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14323 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14324 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14325 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14326 assertNull(calibrator.getEstimatedMa());
14327 assertNull(calibrator.getEstimatedSx());
14328 assertNull(calibrator.getEstimatedSy());
14329 assertNull(calibrator.getEstimatedSz());
14330 assertNull(calibrator.getEstimatedMxy());
14331 assertNull(calibrator.getEstimatedMxz());
14332 assertNull(calibrator.getEstimatedMyx());
14333 assertNull(calibrator.getEstimatedMyz());
14334 assertNull(calibrator.getEstimatedMzx());
14335 assertNull(calibrator.getEstimatedMzy());
14336 assertNull(calibrator.getEstimatedCovariance());
14337 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14338 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14339 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14340 assertNotNull(calibrator.getGroundTruthGravityNorm());
14341 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14342 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14343 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14344 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14345 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14346 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14347 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14348 }
14349
14350 @Test
14351 public void testConstructor119() throws WrongSizeException {
14352 final Collection<StandardDeviationBodyKinematics> measurements =
14353 Collections.emptyList();
14354
14355 final Matrix ba = generateBa();
14356 final double biasX = ba.getElementAtIndex(0);
14357 final double biasY = ba.getElementAtIndex(1);
14358 final double biasZ = ba.getElementAtIndex(2);
14359
14360 final Matrix ma = generateMaCommonAxis();
14361 final double sx = ma.getElementAt(0, 0);
14362 final double sy = ma.getElementAt(1, 1);
14363 final double sz = ma.getElementAt(2, 2);
14364 final double mxy = ma.getElementAt(0, 1);
14365 final double mxz = ma.getElementAt(0, 2);
14366 final double myx = ma.getElementAt(1, 0);
14367 final double myz = ma.getElementAt(1, 2);
14368 final double mzx = ma.getElementAt(2, 0);
14369 final double mzy = ma.getElementAt(2, 1);
14370
14371 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14372 final double latitude = Math.toRadians(
14373 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14374 final double longitude = Math.toRadians(
14375 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14376 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14377 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14378 final NEDVelocity nedVelocity = new NEDVelocity();
14379 final ECEFPosition ecefPosition = new ECEFPosition();
14380 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14381 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14382 ecefPosition, ecefVelocity);
14383
14384 final KnownPositionAccelerometerCalibrator calibrator =
14385 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14386 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14387 myx, myz, mzx, mzy);
14388
14389
14390 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14391 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14392 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14393 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14394 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14395 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14396 final Acceleration bx2 = new Acceleration(0.0,
14397 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14398 calibrator.getInitialBiasXAsAcceleration(bx2);
14399 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14400 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14401 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14402 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14403 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14404 final Acceleration by2 = new Acceleration(0.0,
14405 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14406 calibrator.getInitialBiasYAsAcceleration(by2);
14407 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14408 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14409 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14410 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14411 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14412 final Acceleration bz2 = new Acceleration(0.0,
14413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14414 calibrator.getInitialBiasZAsAcceleration(bz2);
14415 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14416 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14417 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14418 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14419 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14420 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14421 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14422 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14423 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14424 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14425 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14426 final double[] bias1 = calibrator.getInitialBias();
14427 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14428 final double[] bias2 = new double[3];
14429 calibrator.getInitialBias(bias2);
14430 assertArrayEquals(bias1, bias2, 0.0);
14431 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14432 assertEquals(b1, ba);
14433 final Matrix b2 = new Matrix(3, 1);
14434 calibrator.getInitialBiasAsMatrix(b2);
14435 assertEquals(b1, b2);
14436 final Matrix ma1 = new Matrix(3, 3);
14437 ma1.setSubmatrix(0, 0,
14438 2, 2,
14439 new double[]{sx, myx, mzx,
14440 mxy, sy, mzy,
14441 mxz, myz, sz});
14442 assertEquals(calibrator.getInitialMa(), ma1);
14443 final Matrix ma2 = new Matrix(3, 3);
14444 calibrator.getInitialMa(ma2);
14445 assertEquals(ma1, ma2);
14446 assertSame(calibrator.getEcefPosition(), ecefPosition);
14447 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14448 final NEDPosition nedPosition1 = new NEDPosition();
14449 assertTrue(calibrator.getNedPosition(nedPosition1));
14450 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14451 assertSame(calibrator.getMeasurements(), measurements);
14452 assertFalse(calibrator.isCommonAxisUsed());
14453 assertNull(calibrator.getListener());
14454 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14455 assertFalse(calibrator.isReady());
14456 assertFalse(calibrator.isRunning());
14457 assertNull(calibrator.getEstimatedBiases());
14458 assertFalse(calibrator.getEstimatedBiases(null));
14459 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14460 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14461 assertNull(calibrator.getEstimatedBiasFx());
14462 assertNull(calibrator.getEstimatedBiasFy());
14463 assertNull(calibrator.getEstimatedBiasFz());
14464 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14465 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14466 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14467 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14468 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14469 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14470 assertNull(calibrator.getEstimatedMa());
14471 assertNull(calibrator.getEstimatedSx());
14472 assertNull(calibrator.getEstimatedSy());
14473 assertNull(calibrator.getEstimatedSz());
14474 assertNull(calibrator.getEstimatedMxy());
14475 assertNull(calibrator.getEstimatedMxz());
14476 assertNull(calibrator.getEstimatedMyx());
14477 assertNull(calibrator.getEstimatedMyz());
14478 assertNull(calibrator.getEstimatedMzx());
14479 assertNull(calibrator.getEstimatedMzy());
14480 assertNull(calibrator.getEstimatedCovariance());
14481 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14482 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14483 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14484 assertNotNull(calibrator.getGroundTruthGravityNorm());
14485 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14486 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14487 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14488 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14489 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14490 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14491 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14492 }
14493
14494 @Test
14495 public void testConstructor120() throws WrongSizeException {
14496 final Collection<StandardDeviationBodyKinematics> measurements =
14497 Collections.emptyList();
14498
14499 final Matrix ba = generateBa();
14500 final double biasX = ba.getElementAtIndex(0);
14501 final double biasY = ba.getElementAtIndex(1);
14502 final double biasZ = ba.getElementAtIndex(2);
14503
14504 final Matrix ma = generateMaCommonAxis();
14505 final double sx = ma.getElementAt(0, 0);
14506 final double sy = ma.getElementAt(1, 1);
14507 final double sz = ma.getElementAt(2, 2);
14508 final double mxy = ma.getElementAt(0, 1);
14509 final double mxz = ma.getElementAt(0, 2);
14510 final double myx = ma.getElementAt(1, 0);
14511 final double myz = ma.getElementAt(1, 2);
14512 final double mzx = ma.getElementAt(2, 0);
14513 final double mzy = ma.getElementAt(2, 1);
14514
14515 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14516 final double latitude = Math.toRadians(
14517 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14518 final double longitude = Math.toRadians(
14519 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14520 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14521 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14522 final NEDVelocity nedVelocity = new NEDVelocity();
14523 final ECEFPosition ecefPosition = new ECEFPosition();
14524 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14525 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14526 ecefPosition, ecefVelocity);
14527
14528 final KnownPositionAccelerometerCalibrator calibrator =
14529 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14530 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14531 myx, myz, mzx, mzy, this);
14532
14533
14534 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14535 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14536 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14537 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14538 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14539 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14540 final Acceleration bx2 = new Acceleration(0.0,
14541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14542 calibrator.getInitialBiasXAsAcceleration(bx2);
14543 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14544 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14545 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14546 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14547 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14548 final Acceleration by2 = new Acceleration(0.0,
14549 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14550 calibrator.getInitialBiasYAsAcceleration(by2);
14551 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14552 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14553 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14554 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14555 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14556 final Acceleration bz2 = new Acceleration(0.0,
14557 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14558 calibrator.getInitialBiasZAsAcceleration(bz2);
14559 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14560 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14561 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14562 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14563 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14564 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14565 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14566 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14567 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14568 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14569 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14570 final double[] bias1 = calibrator.getInitialBias();
14571 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14572 final double[] bias2 = new double[3];
14573 calibrator.getInitialBias(bias2);
14574 assertArrayEquals(bias1, bias2, 0.0);
14575 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14576 assertEquals(b1, ba);
14577 final Matrix b2 = new Matrix(3, 1);
14578 calibrator.getInitialBiasAsMatrix(b2);
14579 assertEquals(b1, b2);
14580 final Matrix ma1 = new Matrix(3, 3);
14581 ma1.setSubmatrix(0, 0,
14582 2, 2,
14583 new double[]{sx, myx, mzx,
14584 mxy, sy, mzy,
14585 mxz, myz, sz});
14586 assertEquals(calibrator.getInitialMa(), ma1);
14587 final Matrix ma2 = new Matrix(3, 3);
14588 calibrator.getInitialMa(ma2);
14589 assertEquals(ma1, ma2);
14590 assertSame(calibrator.getEcefPosition(), ecefPosition);
14591 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14592 final NEDPosition nedPosition1 = new NEDPosition();
14593 assertTrue(calibrator.getNedPosition(nedPosition1));
14594 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14595 assertSame(calibrator.getMeasurements(), measurements);
14596 assertFalse(calibrator.isCommonAxisUsed());
14597 assertSame(calibrator.getListener(), this);
14598 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
14599 assertFalse(calibrator.isReady());
14600 assertFalse(calibrator.isRunning());
14601 assertNull(calibrator.getEstimatedBiases());
14602 assertFalse(calibrator.getEstimatedBiases(null));
14603 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14604 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14605 assertNull(calibrator.getEstimatedBiasFx());
14606 assertNull(calibrator.getEstimatedBiasFy());
14607 assertNull(calibrator.getEstimatedBiasFz());
14608 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14609 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14610 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14611 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14612 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14613 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14614 assertNull(calibrator.getEstimatedMa());
14615 assertNull(calibrator.getEstimatedSx());
14616 assertNull(calibrator.getEstimatedSy());
14617 assertNull(calibrator.getEstimatedSz());
14618 assertNull(calibrator.getEstimatedMxy());
14619 assertNull(calibrator.getEstimatedMxz());
14620 assertNull(calibrator.getEstimatedMyx());
14621 assertNull(calibrator.getEstimatedMyz());
14622 assertNull(calibrator.getEstimatedMzx());
14623 assertNull(calibrator.getEstimatedMzy());
14624 assertNull(calibrator.getEstimatedCovariance());
14625 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14626 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14627 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14628 assertNotNull(calibrator.getGroundTruthGravityNorm());
14629 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14630 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14631 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14632 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14633 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14634 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14635 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14636 }
14637
14638 @Test
14639 public void testConstructor121() throws WrongSizeException {
14640 final Matrix ba = generateBa();
14641 final double biasX = ba.getElementAtIndex(0);
14642 final double biasY = ba.getElementAtIndex(1);
14643 final double biasZ = ba.getElementAtIndex(2);
14644
14645 final Matrix ma = generateMaCommonAxis();
14646 final double sx = ma.getElementAt(0, 0);
14647 final double sy = ma.getElementAt(1, 1);
14648 final double sz = ma.getElementAt(2, 2);
14649 final double mxy = ma.getElementAt(0, 1);
14650 final double mxz = ma.getElementAt(0, 2);
14651 final double myx = ma.getElementAt(1, 0);
14652 final double myz = ma.getElementAt(1, 2);
14653 final double mzx = ma.getElementAt(2, 0);
14654 final double mzy = ma.getElementAt(2, 1);
14655
14656 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14657 final double latitude = Math.toRadians(
14658 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14659 final double longitude = Math.toRadians(
14660 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14661 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14662 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14663 final NEDVelocity nedVelocity = new NEDVelocity();
14664 final ECEFPosition ecefPosition = new ECEFPosition();
14665 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14666 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14667 ecefPosition, ecefVelocity);
14668
14669 final KnownPositionAccelerometerCalibrator calibrator =
14670 new KnownPositionAccelerometerCalibrator(ecefPosition,
14671 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14672 myx, myz, mzx, mzy);
14673
14674
14675 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14676 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14677 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14678 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14679 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14680 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14681 final Acceleration bx2 = new Acceleration(0.0,
14682 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14683 calibrator.getInitialBiasXAsAcceleration(bx2);
14684 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14685 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14686 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14687 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14688 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14689 final Acceleration by2 = new Acceleration(0.0,
14690 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14691 calibrator.getInitialBiasYAsAcceleration(by2);
14692 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14693 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14694 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14695 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14696 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14697 final Acceleration bz2 = new Acceleration(0.0,
14698 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14699 calibrator.getInitialBiasZAsAcceleration(bz2);
14700 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14701 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14702 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14703 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14704 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14705 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14706 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14707 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14708 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14709 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14710 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14711 final double[] bias1 = calibrator.getInitialBias();
14712 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14713 final double[] bias2 = new double[3];
14714 calibrator.getInitialBias(bias2);
14715 assertArrayEquals(bias1, bias2, 0.0);
14716 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14717 assertEquals(b1, ba);
14718 final Matrix b2 = new Matrix(3, 1);
14719 calibrator.getInitialBiasAsMatrix(b2);
14720 assertEquals(b1, b2);
14721 final Matrix ma1 = new Matrix(3, 3);
14722 ma1.setSubmatrix(0, 0,
14723 2, 2,
14724 new double[]{sx, myx, mzx,
14725 mxy, sy, mzy,
14726 mxz, myz, sz});
14727 assertEquals(calibrator.getInitialMa(), ma1);
14728 final Matrix ma2 = new Matrix(3, 3);
14729 calibrator.getInitialMa(ma2);
14730 assertEquals(ma1, ma2);
14731 assertSame(calibrator.getEcefPosition(), ecefPosition);
14732 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14733 final NEDPosition nedPosition1 = new NEDPosition();
14734 assertTrue(calibrator.getNedPosition(nedPosition1));
14735 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14736 assertNull(calibrator.getMeasurements());
14737 assertTrue(calibrator.isCommonAxisUsed());
14738 assertNull(calibrator.getListener());
14739 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14740 assertFalse(calibrator.isReady());
14741 assertFalse(calibrator.isRunning());
14742 assertNull(calibrator.getEstimatedBiases());
14743 assertFalse(calibrator.getEstimatedBiases(null));
14744 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14745 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14746 assertNull(calibrator.getEstimatedBiasFx());
14747 assertNull(calibrator.getEstimatedBiasFy());
14748 assertNull(calibrator.getEstimatedBiasFz());
14749 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14750 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14751 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14752 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14753 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14754 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14755 assertNull(calibrator.getEstimatedMa());
14756 assertNull(calibrator.getEstimatedSx());
14757 assertNull(calibrator.getEstimatedSy());
14758 assertNull(calibrator.getEstimatedSz());
14759 assertNull(calibrator.getEstimatedMxy());
14760 assertNull(calibrator.getEstimatedMxz());
14761 assertNull(calibrator.getEstimatedMyx());
14762 assertNull(calibrator.getEstimatedMyz());
14763 assertNull(calibrator.getEstimatedMzx());
14764 assertNull(calibrator.getEstimatedMzy());
14765 assertNull(calibrator.getEstimatedCovariance());
14766 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14767 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14768 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14769 assertNotNull(calibrator.getGroundTruthGravityNorm());
14770 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14771 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14772 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14773 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14774 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14775 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14776 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14777 }
14778
14779 @Test
14780 public void testConstructor122() throws WrongSizeException {
14781 final Matrix ba = generateBa();
14782 final double biasX = ba.getElementAtIndex(0);
14783 final double biasY = ba.getElementAtIndex(1);
14784 final double biasZ = ba.getElementAtIndex(2);
14785
14786 final Matrix ma = generateMaCommonAxis();
14787 final double sx = ma.getElementAt(0, 0);
14788 final double sy = ma.getElementAt(1, 1);
14789 final double sz = ma.getElementAt(2, 2);
14790 final double mxy = ma.getElementAt(0, 1);
14791 final double mxz = ma.getElementAt(0, 2);
14792 final double myx = ma.getElementAt(1, 0);
14793 final double myz = ma.getElementAt(1, 2);
14794 final double mzx = ma.getElementAt(2, 0);
14795 final double mzy = ma.getElementAt(2, 1);
14796
14797 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14798 final double latitude = Math.toRadians(
14799 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14800 final double longitude = Math.toRadians(
14801 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14802 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14803 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14804 final NEDVelocity nedVelocity = new NEDVelocity();
14805 final ECEFPosition ecefPosition = new ECEFPosition();
14806 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14807 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14808 ecefPosition, ecefVelocity);
14809
14810 final KnownPositionAccelerometerCalibrator calibrator =
14811 new KnownPositionAccelerometerCalibrator(ecefPosition,
14812 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14813 myx, myz, mzx, mzy, this);
14814
14815
14816 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14817 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14818 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14819 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14820 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14821 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14822 final Acceleration bx2 = new Acceleration(0.0,
14823 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14824 calibrator.getInitialBiasXAsAcceleration(bx2);
14825 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14826 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14827 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14828 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14829 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14830 final Acceleration by2 = new Acceleration(0.0,
14831 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14832 calibrator.getInitialBiasYAsAcceleration(by2);
14833 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14834 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14835 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14836 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14837 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14838 final Acceleration bz2 = new Acceleration(0.0,
14839 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14840 calibrator.getInitialBiasZAsAcceleration(bz2);
14841 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14842 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14843 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14844 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14845 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14846 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14847 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14848 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14849 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14850 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14851 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14852 final double[] bias1 = calibrator.getInitialBias();
14853 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14854 final double[] bias2 = new double[3];
14855 calibrator.getInitialBias(bias2);
14856 assertArrayEquals(bias1, bias2, 0.0);
14857 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
14858 assertEquals(b1, ba);
14859 final Matrix b2 = new Matrix(3, 1);
14860 calibrator.getInitialBiasAsMatrix(b2);
14861 assertEquals(b1, b2);
14862 final Matrix ma1 = new Matrix(3, 3);
14863 ma1.setSubmatrix(0, 0,
14864 2, 2,
14865 new double[]{sx, myx, mzx,
14866 mxy, sy, mzy,
14867 mxz, myz, sz});
14868 assertEquals(calibrator.getInitialMa(), ma1);
14869 final Matrix ma2 = new Matrix(3, 3);
14870 calibrator.getInitialMa(ma2);
14871 assertEquals(ma1, ma2);
14872 assertSame(calibrator.getEcefPosition(), ecefPosition);
14873 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
14874 final NEDPosition nedPosition1 = new NEDPosition();
14875 assertTrue(calibrator.getNedPosition(nedPosition1));
14876 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
14877 assertNull(calibrator.getMeasurements());
14878 assertTrue(calibrator.isCommonAxisUsed());
14879 assertSame(calibrator.getListener(), this);
14880 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
14881 assertFalse(calibrator.isReady());
14882 assertFalse(calibrator.isRunning());
14883 assertNull(calibrator.getEstimatedBiases());
14884 assertFalse(calibrator.getEstimatedBiases(null));
14885 assertNull(calibrator.getEstimatedBiasesAsMatrix());
14886 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
14887 assertNull(calibrator.getEstimatedBiasFx());
14888 assertNull(calibrator.getEstimatedBiasFy());
14889 assertNull(calibrator.getEstimatedBiasFz());
14890 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
14891 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
14892 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
14893 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
14894 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
14895 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
14896 assertNull(calibrator.getEstimatedMa());
14897 assertNull(calibrator.getEstimatedSx());
14898 assertNull(calibrator.getEstimatedSy());
14899 assertNull(calibrator.getEstimatedSz());
14900 assertNull(calibrator.getEstimatedMxy());
14901 assertNull(calibrator.getEstimatedMxz());
14902 assertNull(calibrator.getEstimatedMyx());
14903 assertNull(calibrator.getEstimatedMyz());
14904 assertNull(calibrator.getEstimatedMzx());
14905 assertNull(calibrator.getEstimatedMzy());
14906 assertNull(calibrator.getEstimatedCovariance());
14907 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
14908 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
14909 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
14910 assertNotNull(calibrator.getGroundTruthGravityNorm());
14911 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
14912 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
14913 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
14914 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
14915 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
14916 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
14917 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
14918 }
14919
14920 @Test
14921 public void testConstructor123() throws WrongSizeException {
14922 final Collection<StandardDeviationBodyKinematics> measurements =
14923 Collections.emptyList();
14924
14925 final Matrix ba = generateBa();
14926 final double biasX = ba.getElementAtIndex(0);
14927 final double biasY = ba.getElementAtIndex(1);
14928 final double biasZ = ba.getElementAtIndex(2);
14929
14930 final Matrix ma = generateMaCommonAxis();
14931 final double sx = ma.getElementAt(0, 0);
14932 final double sy = ma.getElementAt(1, 1);
14933 final double sz = ma.getElementAt(2, 2);
14934 final double mxy = ma.getElementAt(0, 1);
14935 final double mxz = ma.getElementAt(0, 2);
14936 final double myx = ma.getElementAt(1, 0);
14937 final double myz = ma.getElementAt(1, 2);
14938 final double mzx = ma.getElementAt(2, 0);
14939 final double mzy = ma.getElementAt(2, 1);
14940
14941 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
14942 final double latitude = Math.toRadians(
14943 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
14944 final double longitude = Math.toRadians(
14945 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
14946 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
14947 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
14948 final NEDVelocity nedVelocity = new NEDVelocity();
14949 final ECEFPosition ecefPosition = new ECEFPosition();
14950 final ECEFVelocity ecefVelocity = new ECEFVelocity();
14951 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
14952 ecefPosition, ecefVelocity);
14953
14954 final KnownPositionAccelerometerCalibrator calibrator =
14955 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
14956 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
14957 myx, myz, mzx, mzy);
14958
14959
14960 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
14961 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
14962 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
14963 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
14964 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
14965 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14966 final Acceleration bx2 = new Acceleration(0.0,
14967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14968 calibrator.getInitialBiasXAsAcceleration(bx2);
14969 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
14970 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14971 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
14972 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
14973 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14974 final Acceleration by2 = new Acceleration(0.0,
14975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14976 calibrator.getInitialBiasYAsAcceleration(by2);
14977 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
14978 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14979 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
14980 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
14981 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14982 final Acceleration bz2 = new Acceleration(0.0,
14983 AccelerationUnit.FEET_PER_SQUARED_SECOND);
14984 calibrator.getInitialBiasZAsAcceleration(bz2);
14985 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
14986 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
14987 assertEquals(calibrator.getInitialSx(), sx, 0.0);
14988 assertEquals(calibrator.getInitialSy(), sy, 0.0);
14989 assertEquals(calibrator.getInitialSz(), sz, 0.0);
14990 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
14991 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
14992 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
14993 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
14994 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
14995 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
14996 final double[] bias1 = calibrator.getInitialBias();
14997 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
14998 final double[] bias2 = new double[3];
14999 calibrator.getInitialBias(bias2);
15000 assertArrayEquals(bias1, bias2, 0.0);
15001 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15002 assertEquals(b1, ba);
15003 final Matrix b2 = new Matrix(3, 1);
15004 calibrator.getInitialBiasAsMatrix(b2);
15005 assertEquals(b1, b2);
15006 final Matrix ma1 = new Matrix(3, 3);
15007 ma1.setSubmatrix(0, 0,
15008 2, 2,
15009 new double[]{sx, myx, mzx,
15010 mxy, sy, mzy,
15011 mxz, myz, sz});
15012 assertEquals(calibrator.getInitialMa(), ma1);
15013 final Matrix ma2 = new Matrix(3, 3);
15014 calibrator.getInitialMa(ma2);
15015 assertEquals(ma1, ma2);
15016 assertSame(calibrator.getEcefPosition(), ecefPosition);
15017 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15018 final NEDPosition nedPosition1 = new NEDPosition();
15019 assertTrue(calibrator.getNedPosition(nedPosition1));
15020 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15021 assertSame(calibrator.getMeasurements(), measurements);
15022 assertTrue(calibrator.isCommonAxisUsed());
15023 assertNull(calibrator.getListener());
15024 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15025 assertFalse(calibrator.isReady());
15026 assertFalse(calibrator.isRunning());
15027 assertNull(calibrator.getEstimatedBiases());
15028 assertFalse(calibrator.getEstimatedBiases(null));
15029 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15030 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15031 assertNull(calibrator.getEstimatedBiasFx());
15032 assertNull(calibrator.getEstimatedBiasFy());
15033 assertNull(calibrator.getEstimatedBiasFz());
15034 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15035 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15036 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15037 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15038 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15039 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15040 assertNull(calibrator.getEstimatedMa());
15041 assertNull(calibrator.getEstimatedSx());
15042 assertNull(calibrator.getEstimatedSy());
15043 assertNull(calibrator.getEstimatedSz());
15044 assertNull(calibrator.getEstimatedMxy());
15045 assertNull(calibrator.getEstimatedMxz());
15046 assertNull(calibrator.getEstimatedMyx());
15047 assertNull(calibrator.getEstimatedMyz());
15048 assertNull(calibrator.getEstimatedMzx());
15049 assertNull(calibrator.getEstimatedMzy());
15050 assertNull(calibrator.getEstimatedCovariance());
15051 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15052 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15053 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15054 assertNotNull(calibrator.getGroundTruthGravityNorm());
15055 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15056 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15057 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15058 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15059 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15060 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15061 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15062 }
15063
15064 @Test
15065 public void testConstructor124() throws WrongSizeException {
15066 final Collection<StandardDeviationBodyKinematics> measurements =
15067 Collections.emptyList();
15068
15069 final Matrix ba = generateBa();
15070 final double biasX = ba.getElementAtIndex(0);
15071 final double biasY = ba.getElementAtIndex(1);
15072 final double biasZ = ba.getElementAtIndex(2);
15073
15074 final Matrix ma = generateMaCommonAxis();
15075 final double sx = ma.getElementAt(0, 0);
15076 final double sy = ma.getElementAt(1, 1);
15077 final double sz = ma.getElementAt(2, 2);
15078 final double mxy = ma.getElementAt(0, 1);
15079 final double mxz = ma.getElementAt(0, 2);
15080 final double myx = ma.getElementAt(1, 0);
15081 final double myz = ma.getElementAt(1, 2);
15082 final double mzx = ma.getElementAt(2, 0);
15083 final double mzy = ma.getElementAt(2, 1);
15084
15085 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15086 final double latitude = Math.toRadians(
15087 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15088 final double longitude = Math.toRadians(
15089 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15090 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15091 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15092 final NEDVelocity nedVelocity = new NEDVelocity();
15093 final ECEFPosition ecefPosition = new ECEFPosition();
15094 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15095 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15096 ecefPosition, ecefVelocity);
15097
15098 final KnownPositionAccelerometerCalibrator calibrator =
15099 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15100 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
15101 myx, myz, mzx, mzy, this);
15102
15103
15104 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15105 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15106 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15107 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15108 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15109 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15110 final Acceleration bx2 = new Acceleration(0.0,
15111 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15112 calibrator.getInitialBiasXAsAcceleration(bx2);
15113 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15114 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15115 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15116 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15117 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15118 final Acceleration by2 = new Acceleration(0.0,
15119 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15120 calibrator.getInitialBiasYAsAcceleration(by2);
15121 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15122 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15123 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15124 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15125 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15126 final Acceleration bz2 = new Acceleration(0.0,
15127 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15128 calibrator.getInitialBiasZAsAcceleration(bz2);
15129 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15130 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15131 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15132 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15133 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15134 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15135 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15136 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15137 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15138 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15139 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15140 final double[] bias1 = calibrator.getInitialBias();
15141 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15142 final double[] bias2 = new double[3];
15143 calibrator.getInitialBias(bias2);
15144 assertArrayEquals(bias1, bias2, 0.0);
15145 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15146 assertEquals(b1, ba);
15147 final Matrix b2 = new Matrix(3, 1);
15148 calibrator.getInitialBiasAsMatrix(b2);
15149 assertEquals(b1, b2);
15150 final Matrix ma1 = new Matrix(3, 3);
15151 ma1.setSubmatrix(0, 0,
15152 2, 2,
15153 new double[]{sx, myx, mzx,
15154 mxy, sy, mzy,
15155 mxz, myz, sz});
15156 assertEquals(calibrator.getInitialMa(), ma1);
15157 final Matrix ma2 = new Matrix(3, 3);
15158 calibrator.getInitialMa(ma2);
15159 assertEquals(ma1, ma2);
15160 assertSame(calibrator.getEcefPosition(), ecefPosition);
15161 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15162 final NEDPosition nedPosition1 = new NEDPosition();
15163 assertTrue(calibrator.getNedPosition(nedPosition1));
15164 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15165 assertSame(calibrator.getMeasurements(), measurements);
15166 assertTrue(calibrator.isCommonAxisUsed());
15167 assertSame(calibrator.getListener(), this);
15168 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15169 assertFalse(calibrator.isReady());
15170 assertFalse(calibrator.isRunning());
15171 assertNull(calibrator.getEstimatedBiases());
15172 assertFalse(calibrator.getEstimatedBiases(null));
15173 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15174 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15175 assertNull(calibrator.getEstimatedBiasFx());
15176 assertNull(calibrator.getEstimatedBiasFy());
15177 assertNull(calibrator.getEstimatedBiasFz());
15178 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15179 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15180 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15181 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15182 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15183 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15184 assertNull(calibrator.getEstimatedMa());
15185 assertNull(calibrator.getEstimatedSx());
15186 assertNull(calibrator.getEstimatedSy());
15187 assertNull(calibrator.getEstimatedSz());
15188 assertNull(calibrator.getEstimatedMxy());
15189 assertNull(calibrator.getEstimatedMxz());
15190 assertNull(calibrator.getEstimatedMyx());
15191 assertNull(calibrator.getEstimatedMyz());
15192 assertNull(calibrator.getEstimatedMzx());
15193 assertNull(calibrator.getEstimatedMzy());
15194 assertNull(calibrator.getEstimatedCovariance());
15195 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15196 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15197 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15198 assertNotNull(calibrator.getGroundTruthGravityNorm());
15199 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15200 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15201 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15202 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15203 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15204 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15205 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15206 }
15207
15208 @Test
15209 public void testConstructor125() throws WrongSizeException {
15210 final Matrix ba = generateBa();
15211 final double biasX = ba.getElementAtIndex(0);
15212 final double biasY = ba.getElementAtIndex(1);
15213 final double biasZ = ba.getElementAtIndex(2);
15214
15215 final Acceleration bx = new Acceleration(biasX,
15216 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15217 final Acceleration by = new Acceleration(biasY,
15218 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15219 final Acceleration bz = new Acceleration(biasZ,
15220 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15221
15222 final Matrix ma = generateMaCommonAxis();
15223 final double sx = ma.getElementAt(0, 0);
15224 final double sy = ma.getElementAt(1, 1);
15225 final double sz = ma.getElementAt(2, 2);
15226 final double mxy = ma.getElementAt(0, 1);
15227 final double mxz = ma.getElementAt(0, 2);
15228 final double myx = ma.getElementAt(1, 0);
15229 final double myz = ma.getElementAt(1, 2);
15230 final double mzx = ma.getElementAt(2, 0);
15231 final double mzy = ma.getElementAt(2, 1);
15232
15233 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15234 final double latitude = Math.toRadians(
15235 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15236 final double longitude = Math.toRadians(
15237 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15238 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15239 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15240 final NEDVelocity nedVelocity = new NEDVelocity();
15241 final ECEFPosition ecefPosition = new ECEFPosition();
15242 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15243 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15244 ecefPosition, ecefVelocity);
15245
15246 final KnownPositionAccelerometerCalibrator calibrator =
15247 new KnownPositionAccelerometerCalibrator(ecefPosition,
15248 bx, by, bz, sx, sy, sz, mxy, mxz,
15249 myx, myz, mzx, mzy);
15250
15251
15252 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15253 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15254 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15255 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15256 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15257 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15258 final Acceleration bx2 = new Acceleration(0.0,
15259 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15260 calibrator.getInitialBiasXAsAcceleration(bx2);
15261 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15262 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15263 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15264 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15265 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15266 final Acceleration by2 = new Acceleration(0.0,
15267 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15268 calibrator.getInitialBiasYAsAcceleration(by2);
15269 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15270 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15271 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15272 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15273 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15274 final Acceleration bz2 = new Acceleration(0.0,
15275 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15276 calibrator.getInitialBiasZAsAcceleration(bz2);
15277 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15278 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15279 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15280 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15281 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15282 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15283 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15284 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15285 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15286 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15287 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15288 final double[] bias1 = calibrator.getInitialBias();
15289 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15290 final double[] bias2 = new double[3];
15291 calibrator.getInitialBias(bias2);
15292 assertArrayEquals(bias1, bias2, 0.0);
15293 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15294 assertEquals(b1, ba);
15295 final Matrix b2 = new Matrix(3, 1);
15296 calibrator.getInitialBiasAsMatrix(b2);
15297 assertEquals(b1, b2);
15298 final Matrix ma1 = new Matrix(3, 3);
15299 ma1.setSubmatrix(0, 0,
15300 2, 2,
15301 new double[]{sx, myx, mzx,
15302 mxy, sy, mzy,
15303 mxz, myz, sz});
15304 assertEquals(calibrator.getInitialMa(), ma1);
15305 final Matrix ma2 = new Matrix(3, 3);
15306 calibrator.getInitialMa(ma2);
15307 assertEquals(ma1, ma2);
15308 assertSame(calibrator.getEcefPosition(), ecefPosition);
15309 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15310 final NEDPosition nedPosition1 = new NEDPosition();
15311 assertTrue(calibrator.getNedPosition(nedPosition1));
15312 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15313 assertNull(calibrator.getMeasurements());
15314 assertFalse(calibrator.isCommonAxisUsed());
15315 assertNull(calibrator.getListener());
15316 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15317 assertFalse(calibrator.isReady());
15318 assertFalse(calibrator.isRunning());
15319 assertNull(calibrator.getEstimatedBiases());
15320 assertFalse(calibrator.getEstimatedBiases(null));
15321 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15322 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15323 assertNull(calibrator.getEstimatedBiasFx());
15324 assertNull(calibrator.getEstimatedBiasFy());
15325 assertNull(calibrator.getEstimatedBiasFz());
15326 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15327 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15328 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15329 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15330 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15331 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15332 assertNull(calibrator.getEstimatedMa());
15333 assertNull(calibrator.getEstimatedSx());
15334 assertNull(calibrator.getEstimatedSy());
15335 assertNull(calibrator.getEstimatedSz());
15336 assertNull(calibrator.getEstimatedMxy());
15337 assertNull(calibrator.getEstimatedMxz());
15338 assertNull(calibrator.getEstimatedMyx());
15339 assertNull(calibrator.getEstimatedMyz());
15340 assertNull(calibrator.getEstimatedMzx());
15341 assertNull(calibrator.getEstimatedMzy());
15342 assertNull(calibrator.getEstimatedCovariance());
15343 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15344 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15345 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15346 assertNotNull(calibrator.getGroundTruthGravityNorm());
15347 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15348 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15349 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15350 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15351 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15352 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15353 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15354 }
15355
15356 @Test
15357 public void testConstructor126() throws WrongSizeException {
15358 final Matrix ba = generateBa();
15359 final double biasX = ba.getElementAtIndex(0);
15360 final double biasY = ba.getElementAtIndex(1);
15361 final double biasZ = ba.getElementAtIndex(2);
15362
15363 final Acceleration bx = new Acceleration(biasX,
15364 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15365 final Acceleration by = new Acceleration(biasY,
15366 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15367 final Acceleration bz = new Acceleration(biasZ,
15368 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15369
15370 final Matrix ma = generateMaCommonAxis();
15371 final double sx = ma.getElementAt(0, 0);
15372 final double sy = ma.getElementAt(1, 1);
15373 final double sz = ma.getElementAt(2, 2);
15374 final double mxy = ma.getElementAt(0, 1);
15375 final double mxz = ma.getElementAt(0, 2);
15376 final double myx = ma.getElementAt(1, 0);
15377 final double myz = ma.getElementAt(1, 2);
15378 final double mzx = ma.getElementAt(2, 0);
15379 final double mzy = ma.getElementAt(2, 1);
15380
15381 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15382 final double latitude = Math.toRadians(
15383 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15384 final double longitude = Math.toRadians(
15385 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15386 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15387 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15388 final NEDVelocity nedVelocity = new NEDVelocity();
15389 final ECEFPosition ecefPosition = new ECEFPosition();
15390 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15391 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15392 ecefPosition, ecefVelocity);
15393
15394 final KnownPositionAccelerometerCalibrator calibrator =
15395 new KnownPositionAccelerometerCalibrator(ecefPosition,
15396 bx, by, bz, sx, sy, sz, mxy, mxz,
15397 myx, myz, mzx, mzy, this);
15398
15399
15400 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15401 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15402 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15403 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15404 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15405 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15406 final Acceleration bx2 = new Acceleration(0.0,
15407 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15408 calibrator.getInitialBiasXAsAcceleration(bx2);
15409 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15410 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15411 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15412 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15413 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15414 final Acceleration by2 = new Acceleration(0.0,
15415 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15416 calibrator.getInitialBiasYAsAcceleration(by2);
15417 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15418 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15419 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15420 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15421 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15422 final Acceleration bz2 = new Acceleration(0.0,
15423 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15424 calibrator.getInitialBiasZAsAcceleration(bz2);
15425 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15426 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15427 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15428 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15429 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15430 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15431 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15432 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15433 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15434 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15435 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15436 final double[] bias1 = calibrator.getInitialBias();
15437 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15438 final double[] bias2 = new double[3];
15439 calibrator.getInitialBias(bias2);
15440 assertArrayEquals(bias1, bias2, 0.0);
15441 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15442 assertEquals(b1, ba);
15443 final Matrix b2 = new Matrix(3, 1);
15444 calibrator.getInitialBiasAsMatrix(b2);
15445 assertEquals(b1, b2);
15446 final Matrix ma1 = new Matrix(3, 3);
15447 ma1.setSubmatrix(0, 0,
15448 2, 2,
15449 new double[]{sx, myx, mzx,
15450 mxy, sy, mzy,
15451 mxz, myz, sz});
15452 assertEquals(calibrator.getInitialMa(), ma1);
15453 final Matrix ma2 = new Matrix(3, 3);
15454 calibrator.getInitialMa(ma2);
15455 assertEquals(ma1, ma2);
15456 assertSame(calibrator.getEcefPosition(), ecefPosition);
15457 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15458 final NEDPosition nedPosition1 = new NEDPosition();
15459 assertTrue(calibrator.getNedPosition(nedPosition1));
15460 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15461 assertNull(calibrator.getMeasurements());
15462 assertFalse(calibrator.isCommonAxisUsed());
15463 assertSame(calibrator.getListener(), this);
15464 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15465 assertFalse(calibrator.isReady());
15466 assertFalse(calibrator.isRunning());
15467 assertNull(calibrator.getEstimatedBiases());
15468 assertFalse(calibrator.getEstimatedBiases(null));
15469 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15470 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15471 assertNull(calibrator.getEstimatedBiasFx());
15472 assertNull(calibrator.getEstimatedBiasFy());
15473 assertNull(calibrator.getEstimatedBiasFz());
15474 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15475 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15476 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15477 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15478 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15479 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15480 assertNull(calibrator.getEstimatedMa());
15481 assertNull(calibrator.getEstimatedSx());
15482 assertNull(calibrator.getEstimatedSy());
15483 assertNull(calibrator.getEstimatedSz());
15484 assertNull(calibrator.getEstimatedMxy());
15485 assertNull(calibrator.getEstimatedMxz());
15486 assertNull(calibrator.getEstimatedMyx());
15487 assertNull(calibrator.getEstimatedMyz());
15488 assertNull(calibrator.getEstimatedMzx());
15489 assertNull(calibrator.getEstimatedMzy());
15490 assertNull(calibrator.getEstimatedCovariance());
15491 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15492 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15493 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15494 assertNotNull(calibrator.getGroundTruthGravityNorm());
15495 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15496 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15497 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15498 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15499 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15500 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15501 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15502 }
15503
15504 @Test
15505 public void testConstructor127() throws WrongSizeException {
15506 final Collection<StandardDeviationBodyKinematics> measurements =
15507 Collections.emptyList();
15508
15509 final Matrix ba = generateBa();
15510 final double biasX = ba.getElementAtIndex(0);
15511 final double biasY = ba.getElementAtIndex(1);
15512 final double biasZ = ba.getElementAtIndex(2);
15513
15514 final Acceleration bx = new Acceleration(biasX,
15515 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15516 final Acceleration by = new Acceleration(biasY,
15517 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15518 final Acceleration bz = new Acceleration(biasZ,
15519 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15520
15521 final Matrix ma = generateMaCommonAxis();
15522 final double sx = ma.getElementAt(0, 0);
15523 final double sy = ma.getElementAt(1, 1);
15524 final double sz = ma.getElementAt(2, 2);
15525 final double mxy = ma.getElementAt(0, 1);
15526 final double mxz = ma.getElementAt(0, 2);
15527 final double myx = ma.getElementAt(1, 0);
15528 final double myz = ma.getElementAt(1, 2);
15529 final double mzx = ma.getElementAt(2, 0);
15530 final double mzy = ma.getElementAt(2, 1);
15531
15532 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15533 final double latitude = Math.toRadians(
15534 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15535 final double longitude = Math.toRadians(
15536 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15537 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15538 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15539 final NEDVelocity nedVelocity = new NEDVelocity();
15540 final ECEFPosition ecefPosition = new ECEFPosition();
15541 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15542 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15543 ecefPosition, ecefVelocity);
15544
15545 final KnownPositionAccelerometerCalibrator calibrator =
15546 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15547 bx, by, bz, sx, sy, sz, mxy, mxz,
15548 myx, myz, mzx, mzy);
15549
15550
15551 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15552 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15553 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15554 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15555 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15556 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15557 final Acceleration bx2 = new Acceleration(0.0,
15558 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15559 calibrator.getInitialBiasXAsAcceleration(bx2);
15560 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15561 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15562 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15563 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15564 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15565 final Acceleration by2 = new Acceleration(0.0,
15566 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15567 calibrator.getInitialBiasYAsAcceleration(by2);
15568 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15569 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15570 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15571 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15572 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15573 final Acceleration bz2 = new Acceleration(0.0,
15574 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15575 calibrator.getInitialBiasZAsAcceleration(bz2);
15576 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15577 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15578 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15579 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15580 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15581 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15582 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15583 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15584 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15585 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15586 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15587 final double[] bias1 = calibrator.getInitialBias();
15588 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15589 final double[] bias2 = new double[3];
15590 calibrator.getInitialBias(bias2);
15591 assertArrayEquals(bias1, bias2, 0.0);
15592 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15593 assertEquals(b1, ba);
15594 final Matrix b2 = new Matrix(3, 1);
15595 calibrator.getInitialBiasAsMatrix(b2);
15596 assertEquals(b1, b2);
15597 final Matrix ma1 = new Matrix(3, 3);
15598 ma1.setSubmatrix(0, 0,
15599 2, 2,
15600 new double[]{sx, myx, mzx,
15601 mxy, sy, mzy,
15602 mxz, myz, sz});
15603 assertEquals(calibrator.getInitialMa(), ma1);
15604 final Matrix ma2 = new Matrix(3, 3);
15605 calibrator.getInitialMa(ma2);
15606 assertEquals(ma1, ma2);
15607 assertSame(calibrator.getEcefPosition(), ecefPosition);
15608 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15609 final NEDPosition nedPosition1 = new NEDPosition();
15610 assertTrue(calibrator.getNedPosition(nedPosition1));
15611 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15612 assertSame(calibrator.getMeasurements(), measurements);
15613 assertFalse(calibrator.isCommonAxisUsed());
15614 assertNull(calibrator.getListener());
15615 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15616 assertFalse(calibrator.isReady());
15617 assertFalse(calibrator.isRunning());
15618 assertNull(calibrator.getEstimatedBiases());
15619 assertFalse(calibrator.getEstimatedBiases(null));
15620 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15621 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15622 assertNull(calibrator.getEstimatedBiasFx());
15623 assertNull(calibrator.getEstimatedBiasFy());
15624 assertNull(calibrator.getEstimatedBiasFz());
15625 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15626 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15627 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15628 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15629 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15630 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15631 assertNull(calibrator.getEstimatedMa());
15632 assertNull(calibrator.getEstimatedSx());
15633 assertNull(calibrator.getEstimatedSy());
15634 assertNull(calibrator.getEstimatedSz());
15635 assertNull(calibrator.getEstimatedMxy());
15636 assertNull(calibrator.getEstimatedMxz());
15637 assertNull(calibrator.getEstimatedMyx());
15638 assertNull(calibrator.getEstimatedMyz());
15639 assertNull(calibrator.getEstimatedMzx());
15640 assertNull(calibrator.getEstimatedMzy());
15641 assertNull(calibrator.getEstimatedCovariance());
15642 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15643 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15644 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15645 assertNotNull(calibrator.getGroundTruthGravityNorm());
15646 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15647 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15648 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15649 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15650 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15651 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15652 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15653 }
15654
15655 @Test
15656 public void testConstructor128() throws WrongSizeException {
15657 final Collection<StandardDeviationBodyKinematics> measurements =
15658 Collections.emptyList();
15659
15660 final Matrix ba = generateBa();
15661 final double biasX = ba.getElementAtIndex(0);
15662 final double biasY = ba.getElementAtIndex(1);
15663 final double biasZ = ba.getElementAtIndex(2);
15664
15665 final Acceleration bx = new Acceleration(biasX,
15666 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15667 final Acceleration by = new Acceleration(biasY,
15668 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15669 final Acceleration bz = new Acceleration(biasZ,
15670 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15671
15672 final Matrix ma = generateMaCommonAxis();
15673 final double sx = ma.getElementAt(0, 0);
15674 final double sy = ma.getElementAt(1, 1);
15675 final double sz = ma.getElementAt(2, 2);
15676 final double mxy = ma.getElementAt(0, 1);
15677 final double mxz = ma.getElementAt(0, 2);
15678 final double myx = ma.getElementAt(1, 0);
15679 final double myz = ma.getElementAt(1, 2);
15680 final double mzx = ma.getElementAt(2, 0);
15681 final double mzy = ma.getElementAt(2, 1);
15682
15683 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15684 final double latitude = Math.toRadians(
15685 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15686 final double longitude = Math.toRadians(
15687 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15688 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15689 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15690 final NEDVelocity nedVelocity = new NEDVelocity();
15691 final ECEFPosition ecefPosition = new ECEFPosition();
15692 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15693 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15694 ecefPosition, ecefVelocity);
15695
15696 final KnownPositionAccelerometerCalibrator calibrator =
15697 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
15698 bx, by, bz, sx, sy, sz, mxy, mxz,
15699 myx, myz, mzx, mzy, this);
15700
15701
15702 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15703 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15704 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15705 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15706 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15707 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15708 final Acceleration bx2 = new Acceleration(0.0,
15709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15710 calibrator.getInitialBiasXAsAcceleration(bx2);
15711 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15712 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15713 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15714 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15715 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15716 final Acceleration by2 = new Acceleration(0.0,
15717 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15718 calibrator.getInitialBiasYAsAcceleration(by2);
15719 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15720 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15721 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15722 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15723 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15724 final Acceleration bz2 = new Acceleration(0.0,
15725 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15726 calibrator.getInitialBiasZAsAcceleration(bz2);
15727 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15728 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15729 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15730 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15731 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15732 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15733 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15734 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15735 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15736 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15737 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15738 final double[] bias1 = calibrator.getInitialBias();
15739 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15740 final double[] bias2 = new double[3];
15741 calibrator.getInitialBias(bias2);
15742 assertArrayEquals(bias1, bias2, 0.0);
15743 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15744 assertEquals(b1, ba);
15745 final Matrix b2 = new Matrix(3, 1);
15746 calibrator.getInitialBiasAsMatrix(b2);
15747 assertEquals(b1, b2);
15748 final Matrix ma1 = new Matrix(3, 3);
15749 ma1.setSubmatrix(0, 0,
15750 2, 2,
15751 new double[]{sx, myx, mzx,
15752 mxy, sy, mzy,
15753 mxz, myz, sz});
15754 assertEquals(calibrator.getInitialMa(), ma1);
15755 final Matrix ma2 = new Matrix(3, 3);
15756 calibrator.getInitialMa(ma2);
15757 assertEquals(ma1, ma2);
15758 assertSame(calibrator.getEcefPosition(), ecefPosition);
15759 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15760 final NEDPosition nedPosition1 = new NEDPosition();
15761 assertTrue(calibrator.getNedPosition(nedPosition1));
15762 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15763 assertSame(calibrator.getMeasurements(), measurements);
15764 assertFalse(calibrator.isCommonAxisUsed());
15765 assertSame(calibrator.getListener(), this);
15766 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
15767 assertFalse(calibrator.isReady());
15768 assertFalse(calibrator.isRunning());
15769 assertNull(calibrator.getEstimatedBiases());
15770 assertFalse(calibrator.getEstimatedBiases(null));
15771 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15772 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15773 assertNull(calibrator.getEstimatedBiasFx());
15774 assertNull(calibrator.getEstimatedBiasFy());
15775 assertNull(calibrator.getEstimatedBiasFz());
15776 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15777 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15778 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15779 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15780 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15781 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15782 assertNull(calibrator.getEstimatedMa());
15783 assertNull(calibrator.getEstimatedSx());
15784 assertNull(calibrator.getEstimatedSy());
15785 assertNull(calibrator.getEstimatedSz());
15786 assertNull(calibrator.getEstimatedMxy());
15787 assertNull(calibrator.getEstimatedMxz());
15788 assertNull(calibrator.getEstimatedMyx());
15789 assertNull(calibrator.getEstimatedMyz());
15790 assertNull(calibrator.getEstimatedMzx());
15791 assertNull(calibrator.getEstimatedMzy());
15792 assertNull(calibrator.getEstimatedCovariance());
15793 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15794 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15795 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15796 assertNotNull(calibrator.getGroundTruthGravityNorm());
15797 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15798 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15799 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15800 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15801 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15802 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15803 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15804 }
15805
15806 @Test
15807 public void testConstructor129() throws WrongSizeException {
15808 final Matrix ba = generateBa();
15809 final double biasX = ba.getElementAtIndex(0);
15810 final double biasY = ba.getElementAtIndex(1);
15811 final double biasZ = ba.getElementAtIndex(2);
15812
15813 final Acceleration bx = new Acceleration(biasX,
15814 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15815 final Acceleration by = new Acceleration(biasY,
15816 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15817 final Acceleration bz = new Acceleration(biasZ,
15818 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15819
15820 final Matrix ma = generateMaCommonAxis();
15821 final double sx = ma.getElementAt(0, 0);
15822 final double sy = ma.getElementAt(1, 1);
15823 final double sz = ma.getElementAt(2, 2);
15824 final double mxy = ma.getElementAt(0, 1);
15825 final double mxz = ma.getElementAt(0, 2);
15826 final double myx = ma.getElementAt(1, 0);
15827 final double myz = ma.getElementAt(1, 2);
15828 final double mzx = ma.getElementAt(2, 0);
15829 final double mzy = ma.getElementAt(2, 1);
15830
15831 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15832 final double latitude = Math.toRadians(
15833 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15834 final double longitude = Math.toRadians(
15835 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15836 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15837 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15838 final NEDVelocity nedVelocity = new NEDVelocity();
15839 final ECEFPosition ecefPosition = new ECEFPosition();
15840 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15841 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15842 ecefPosition, ecefVelocity);
15843
15844 final KnownPositionAccelerometerCalibrator calibrator =
15845 new KnownPositionAccelerometerCalibrator(ecefPosition,
15846 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15847 myx, myz, mzx, mzy);
15848
15849
15850 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15851 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
15852 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
15853 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
15854 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
15855 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15856 final Acceleration bx2 = new Acceleration(0.0,
15857 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15858 calibrator.getInitialBiasXAsAcceleration(bx2);
15859 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
15860 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15861 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
15862 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
15863 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15864 final Acceleration by2 = new Acceleration(0.0,
15865 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15866 calibrator.getInitialBiasYAsAcceleration(by2);
15867 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
15868 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15869 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
15870 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
15871 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15872 final Acceleration bz2 = new Acceleration(0.0,
15873 AccelerationUnit.FEET_PER_SQUARED_SECOND);
15874 calibrator.getInitialBiasZAsAcceleration(bz2);
15875 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
15876 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
15877 assertEquals(calibrator.getInitialSx(), sx, 0.0);
15878 assertEquals(calibrator.getInitialSy(), sy, 0.0);
15879 assertEquals(calibrator.getInitialSz(), sz, 0.0);
15880 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
15881 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
15882 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
15883 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
15884 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
15885 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
15886 final double[] bias1 = calibrator.getInitialBias();
15887 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
15888 final double[] bias2 = new double[3];
15889 calibrator.getInitialBias(bias2);
15890 assertArrayEquals(bias1, bias2, 0.0);
15891 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
15892 assertEquals(b1, ba);
15893 final Matrix b2 = new Matrix(3, 1);
15894 calibrator.getInitialBiasAsMatrix(b2);
15895 assertEquals(b1, b2);
15896 final Matrix ma1 = new Matrix(3, 3);
15897 ma1.setSubmatrix(0, 0,
15898 2, 2,
15899 new double[]{sx, myx, mzx,
15900 mxy, sy, mzy,
15901 mxz, myz, sz});
15902 assertEquals(calibrator.getInitialMa(), ma1);
15903 final Matrix ma2 = new Matrix(3, 3);
15904 calibrator.getInitialMa(ma2);
15905 assertEquals(ma1, ma2);
15906 assertSame(calibrator.getEcefPosition(), ecefPosition);
15907 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
15908 final NEDPosition nedPosition1 = new NEDPosition();
15909 assertTrue(calibrator.getNedPosition(nedPosition1));
15910 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
15911 assertNull(calibrator.getMeasurements());
15912 assertTrue(calibrator.isCommonAxisUsed());
15913 assertNull(calibrator.getListener());
15914 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
15915 assertFalse(calibrator.isReady());
15916 assertFalse(calibrator.isRunning());
15917 assertNull(calibrator.getEstimatedBiases());
15918 assertFalse(calibrator.getEstimatedBiases(null));
15919 assertNull(calibrator.getEstimatedBiasesAsMatrix());
15920 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
15921 assertNull(calibrator.getEstimatedBiasFx());
15922 assertNull(calibrator.getEstimatedBiasFy());
15923 assertNull(calibrator.getEstimatedBiasFz());
15924 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
15925 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
15926 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
15927 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
15928 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
15929 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
15930 assertNull(calibrator.getEstimatedMa());
15931 assertNull(calibrator.getEstimatedSx());
15932 assertNull(calibrator.getEstimatedSy());
15933 assertNull(calibrator.getEstimatedSz());
15934 assertNull(calibrator.getEstimatedMxy());
15935 assertNull(calibrator.getEstimatedMxz());
15936 assertNull(calibrator.getEstimatedMyx());
15937 assertNull(calibrator.getEstimatedMyz());
15938 assertNull(calibrator.getEstimatedMzx());
15939 assertNull(calibrator.getEstimatedMzy());
15940 assertNull(calibrator.getEstimatedCovariance());
15941 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
15942 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
15943 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
15944 assertNotNull(calibrator.getGroundTruthGravityNorm());
15945 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
15946 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
15947 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
15948 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
15949 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
15950 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
15951 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
15952 }
15953
15954 @Test
15955 public void testConstructor130() throws WrongSizeException {
15956 final Matrix ba = generateBa();
15957 final double biasX = ba.getElementAtIndex(0);
15958 final double biasY = ba.getElementAtIndex(1);
15959 final double biasZ = ba.getElementAtIndex(2);
15960
15961 final Acceleration bx = new Acceleration(biasX,
15962 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15963 final Acceleration by = new Acceleration(biasY,
15964 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15965 final Acceleration bz = new Acceleration(biasZ,
15966 AccelerationUnit.METERS_PER_SQUARED_SECOND);
15967
15968 final Matrix ma = generateMaCommonAxis();
15969 final double sx = ma.getElementAt(0, 0);
15970 final double sy = ma.getElementAt(1, 1);
15971 final double sz = ma.getElementAt(2, 2);
15972 final double mxy = ma.getElementAt(0, 1);
15973 final double mxz = ma.getElementAt(0, 2);
15974 final double myx = ma.getElementAt(1, 0);
15975 final double myz = ma.getElementAt(1, 2);
15976 final double mzx = ma.getElementAt(2, 0);
15977 final double mzy = ma.getElementAt(2, 1);
15978
15979 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
15980 final double latitude = Math.toRadians(
15981 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
15982 final double longitude = Math.toRadians(
15983 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
15984 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
15985 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
15986 final NEDVelocity nedVelocity = new NEDVelocity();
15987 final ECEFPosition ecefPosition = new ECEFPosition();
15988 final ECEFVelocity ecefVelocity = new ECEFVelocity();
15989 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
15990 ecefPosition, ecefVelocity);
15991
15992 final KnownPositionAccelerometerCalibrator calibrator =
15993 new KnownPositionAccelerometerCalibrator(ecefPosition,
15994 true, bx, by, bz, sx, sy, sz, mxy, mxz,
15995 myx, myz, mzx, mzy, this);
15996
15997
15998 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
15999 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16000 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16001 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16002 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16003 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16004 final Acceleration bx2 = new Acceleration(0.0,
16005 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16006 calibrator.getInitialBiasXAsAcceleration(bx2);
16007 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16008 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16009 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16010 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16011 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16012 final Acceleration by2 = new Acceleration(0.0,
16013 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16014 calibrator.getInitialBiasYAsAcceleration(by2);
16015 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16016 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16017 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16018 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16019 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16020 final Acceleration bz2 = new Acceleration(0.0,
16021 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16022 calibrator.getInitialBiasZAsAcceleration(bz2);
16023 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16024 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16025 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16026 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16027 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16028 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16029 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16030 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16031 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16032 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16033 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16034 final double[] bias1 = calibrator.getInitialBias();
16035 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16036 final double[] bias2 = new double[3];
16037 calibrator.getInitialBias(bias2);
16038 assertArrayEquals(bias1, bias2, 0.0);
16039 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16040 assertEquals(b1, ba);
16041 final Matrix b2 = new Matrix(3, 1);
16042 calibrator.getInitialBiasAsMatrix(b2);
16043 assertEquals(b1, b2);
16044 final Matrix ma1 = new Matrix(3, 3);
16045 ma1.setSubmatrix(0, 0,
16046 2, 2,
16047 new double[]{sx, myx, mzx,
16048 mxy, sy, mzy,
16049 mxz, myz, sz});
16050 assertEquals(calibrator.getInitialMa(), ma1);
16051 final Matrix ma2 = new Matrix(3, 3);
16052 calibrator.getInitialMa(ma2);
16053 assertEquals(ma1, ma2);
16054 assertSame(calibrator.getEcefPosition(), ecefPosition);
16055 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16056 final NEDPosition nedPosition1 = new NEDPosition();
16057 assertTrue(calibrator.getNedPosition(nedPosition1));
16058 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16059 assertNull(calibrator.getMeasurements());
16060 assertTrue(calibrator.isCommonAxisUsed());
16061 assertSame(calibrator.getListener(), this);
16062 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16063 assertFalse(calibrator.isReady());
16064 assertFalse(calibrator.isRunning());
16065 assertNull(calibrator.getEstimatedBiases());
16066 assertFalse(calibrator.getEstimatedBiases(null));
16067 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16068 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16069 assertNull(calibrator.getEstimatedBiasFx());
16070 assertNull(calibrator.getEstimatedBiasFy());
16071 assertNull(calibrator.getEstimatedBiasFz());
16072 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16073 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16074 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16075 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16076 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16077 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16078 assertNull(calibrator.getEstimatedMa());
16079 assertNull(calibrator.getEstimatedSx());
16080 assertNull(calibrator.getEstimatedSy());
16081 assertNull(calibrator.getEstimatedSz());
16082 assertNull(calibrator.getEstimatedMxy());
16083 assertNull(calibrator.getEstimatedMxz());
16084 assertNull(calibrator.getEstimatedMyx());
16085 assertNull(calibrator.getEstimatedMyz());
16086 assertNull(calibrator.getEstimatedMzx());
16087 assertNull(calibrator.getEstimatedMzy());
16088 assertNull(calibrator.getEstimatedCovariance());
16089 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16090 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16091 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16092 assertNotNull(calibrator.getGroundTruthGravityNorm());
16093 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16094 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16095 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16096 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16097 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16098 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16099 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16100 }
16101
16102 @Test
16103 public void testConstructor131() throws WrongSizeException {
16104 final Collection<StandardDeviationBodyKinematics> measurements =
16105 Collections.emptyList();
16106
16107 final Matrix ba = generateBa();
16108 final double biasX = ba.getElementAtIndex(0);
16109 final double biasY = ba.getElementAtIndex(1);
16110 final double biasZ = ba.getElementAtIndex(2);
16111
16112 final Acceleration bx = new Acceleration(biasX,
16113 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16114 final Acceleration by = new Acceleration(biasY,
16115 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16116 final Acceleration bz = new Acceleration(biasZ,
16117 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16118
16119 final Matrix ma = generateMaCommonAxis();
16120 final double sx = ma.getElementAt(0, 0);
16121 final double sy = ma.getElementAt(1, 1);
16122 final double sz = ma.getElementAt(2, 2);
16123 final double mxy = ma.getElementAt(0, 1);
16124 final double mxz = ma.getElementAt(0, 2);
16125 final double myx = ma.getElementAt(1, 0);
16126 final double myz = ma.getElementAt(1, 2);
16127 final double mzx = ma.getElementAt(2, 0);
16128 final double mzy = ma.getElementAt(2, 1);
16129
16130 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16131 final double latitude = Math.toRadians(
16132 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16133 final double longitude = Math.toRadians(
16134 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16135 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16136 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16137 final NEDVelocity nedVelocity = new NEDVelocity();
16138 final ECEFPosition ecefPosition = new ECEFPosition();
16139 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16140 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16141 ecefPosition, ecefVelocity);
16142
16143 final KnownPositionAccelerometerCalibrator calibrator =
16144 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16145 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16146 myx, myz, mzx, mzy);
16147
16148
16149 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16150 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16151 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16152 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16153 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16154 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16155 final Acceleration bx2 = new Acceleration(0.0,
16156 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16157 calibrator.getInitialBiasXAsAcceleration(bx2);
16158 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16159 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16160 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16161 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16162 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16163 final Acceleration by2 = new Acceleration(0.0,
16164 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16165 calibrator.getInitialBiasYAsAcceleration(by2);
16166 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16167 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16168 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16169 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16170 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16171 final Acceleration bz2 = new Acceleration(0.0,
16172 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16173 calibrator.getInitialBiasZAsAcceleration(bz2);
16174 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16175 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16176 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16177 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16178 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16179 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16180 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16181 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16182 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16183 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16184 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16185 final double[] bias1 = calibrator.getInitialBias();
16186 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16187 final double[] bias2 = new double[3];
16188 calibrator.getInitialBias(bias2);
16189 assertArrayEquals(bias1, bias2, 0.0);
16190 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16191 assertEquals(b1, ba);
16192 final Matrix b2 = new Matrix(3, 1);
16193 calibrator.getInitialBiasAsMatrix(b2);
16194 assertEquals(b1, b2);
16195 final Matrix ma1 = new Matrix(3, 3);
16196 ma1.setSubmatrix(0, 0,
16197 2, 2,
16198 new double[]{sx, myx, mzx,
16199 mxy, sy, mzy,
16200 mxz, myz, sz});
16201 assertEquals(calibrator.getInitialMa(), ma1);
16202 final Matrix ma2 = new Matrix(3, 3);
16203 calibrator.getInitialMa(ma2);
16204 assertEquals(ma1, ma2);
16205 assertSame(calibrator.getEcefPosition(), ecefPosition);
16206 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16207 final NEDPosition nedPosition1 = new NEDPosition();
16208 assertTrue(calibrator.getNedPosition(nedPosition1));
16209 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16210 assertSame(calibrator.getMeasurements(), measurements);
16211 assertTrue(calibrator.isCommonAxisUsed());
16212 assertNull(calibrator.getListener());
16213 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16214 assertFalse(calibrator.isReady());
16215 assertFalse(calibrator.isRunning());
16216 assertNull(calibrator.getEstimatedBiases());
16217 assertFalse(calibrator.getEstimatedBiases(null));
16218 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16219 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16220 assertNull(calibrator.getEstimatedBiasFx());
16221 assertNull(calibrator.getEstimatedBiasFy());
16222 assertNull(calibrator.getEstimatedBiasFz());
16223 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16224 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16225 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16226 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16227 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16228 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16229 assertNull(calibrator.getEstimatedMa());
16230 assertNull(calibrator.getEstimatedSx());
16231 assertNull(calibrator.getEstimatedSy());
16232 assertNull(calibrator.getEstimatedSz());
16233 assertNull(calibrator.getEstimatedMxy());
16234 assertNull(calibrator.getEstimatedMxz());
16235 assertNull(calibrator.getEstimatedMyx());
16236 assertNull(calibrator.getEstimatedMyz());
16237 assertNull(calibrator.getEstimatedMzx());
16238 assertNull(calibrator.getEstimatedMzy());
16239 assertNull(calibrator.getEstimatedCovariance());
16240 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16241 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16242 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16243 assertNotNull(calibrator.getGroundTruthGravityNorm());
16244 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16245 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16246 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16247 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16248 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16249 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16250 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16251 }
16252
16253 @Test
16254 public void testConstructor132() throws WrongSizeException {
16255 final Collection<StandardDeviationBodyKinematics> measurements =
16256 Collections.emptyList();
16257
16258 final Matrix ba = generateBa();
16259 final double biasX = ba.getElementAtIndex(0);
16260 final double biasY = ba.getElementAtIndex(1);
16261 final double biasZ = ba.getElementAtIndex(2);
16262
16263 final Acceleration bx = new Acceleration(biasX,
16264 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16265 final Acceleration by = new Acceleration(biasY,
16266 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16267 final Acceleration bz = new Acceleration(biasZ,
16268 AccelerationUnit.METERS_PER_SQUARED_SECOND);
16269
16270 final Matrix ma = generateMaCommonAxis();
16271 final double sx = ma.getElementAt(0, 0);
16272 final double sy = ma.getElementAt(1, 1);
16273 final double sz = ma.getElementAt(2, 2);
16274 final double mxy = ma.getElementAt(0, 1);
16275 final double mxz = ma.getElementAt(0, 2);
16276 final double myx = ma.getElementAt(1, 0);
16277 final double myz = ma.getElementAt(1, 2);
16278 final double mzx = ma.getElementAt(2, 0);
16279 final double mzy = ma.getElementAt(2, 1);
16280
16281 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16282 final double latitude = Math.toRadians(
16283 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16284 final double longitude = Math.toRadians(
16285 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16286 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16287 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16288 final NEDVelocity nedVelocity = new NEDVelocity();
16289 final ECEFPosition ecefPosition = new ECEFPosition();
16290 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16291 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16292 ecefPosition, ecefVelocity);
16293
16294 final KnownPositionAccelerometerCalibrator calibrator =
16295 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
16296 true, bx, by, bz, sx, sy, sz, mxy, mxz,
16297 myx, myz, mzx, mzy, this);
16298
16299
16300 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16301 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16302 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16303 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16304 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16305 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16306 final Acceleration bx2 = new Acceleration(0.0,
16307 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16308 calibrator.getInitialBiasXAsAcceleration(bx2);
16309 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16310 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16311 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16312 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16313 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16314 final Acceleration by2 = new Acceleration(0.0,
16315 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16316 calibrator.getInitialBiasYAsAcceleration(by2);
16317 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16318 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16319 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16320 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16321 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16322 final Acceleration bz2 = new Acceleration(0.0,
16323 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16324 calibrator.getInitialBiasZAsAcceleration(bz2);
16325 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16326 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16327 assertEquals(calibrator.getInitialSx(), sx, 0.0);
16328 assertEquals(calibrator.getInitialSy(), sy, 0.0);
16329 assertEquals(calibrator.getInitialSz(), sz, 0.0);
16330 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
16331 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
16332 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
16333 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
16334 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
16335 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
16336 final double[] bias1 = calibrator.getInitialBias();
16337 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
16338 final double[] bias2 = new double[3];
16339 calibrator.getInitialBias(bias2);
16340 assertArrayEquals(bias1, bias2, 0.0);
16341 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16342 assertEquals(b1, ba);
16343 final Matrix b2 = new Matrix(3, 1);
16344 calibrator.getInitialBiasAsMatrix(b2);
16345 assertEquals(b1, b2);
16346 final Matrix ma1 = new Matrix(3, 3);
16347 ma1.setSubmatrix(0, 0,
16348 2, 2,
16349 new double[]{sx, myx, mzx,
16350 mxy, sy, mzy,
16351 mxz, myz, sz});
16352 assertEquals(calibrator.getInitialMa(), ma1);
16353 final Matrix ma2 = new Matrix(3, 3);
16354 calibrator.getInitialMa(ma2);
16355 assertEquals(ma1, ma2);
16356 assertSame(calibrator.getEcefPosition(), ecefPosition);
16357 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16358 final NEDPosition nedPosition1 = new NEDPosition();
16359 assertTrue(calibrator.getNedPosition(nedPosition1));
16360 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16361 assertSame(calibrator.getMeasurements(), measurements);
16362 assertTrue(calibrator.isCommonAxisUsed());
16363 assertSame(calibrator.getListener(), this);
16364 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
16365 assertFalse(calibrator.isReady());
16366 assertFalse(calibrator.isRunning());
16367 assertNull(calibrator.getEstimatedBiases());
16368 assertFalse(calibrator.getEstimatedBiases(null));
16369 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16370 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16371 assertNull(calibrator.getEstimatedBiasFx());
16372 assertNull(calibrator.getEstimatedBiasFy());
16373 assertNull(calibrator.getEstimatedBiasFz());
16374 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16375 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16376 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16377 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16378 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16379 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16380 assertNull(calibrator.getEstimatedMa());
16381 assertNull(calibrator.getEstimatedSx());
16382 assertNull(calibrator.getEstimatedSy());
16383 assertNull(calibrator.getEstimatedSz());
16384 assertNull(calibrator.getEstimatedMxy());
16385 assertNull(calibrator.getEstimatedMxz());
16386 assertNull(calibrator.getEstimatedMyx());
16387 assertNull(calibrator.getEstimatedMyz());
16388 assertNull(calibrator.getEstimatedMzx());
16389 assertNull(calibrator.getEstimatedMzy());
16390 assertNull(calibrator.getEstimatedCovariance());
16391 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16392 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16393 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16394 assertNotNull(calibrator.getGroundTruthGravityNorm());
16395 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16396 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16397 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16398 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16399 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16400 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16401 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16402 }
16403
16404 @Test
16405 public void testConstructor133() throws WrongSizeException {
16406 final Matrix ba = generateBa();
16407 final double[] bias = ba.getBuffer();
16408 final double biasX = ba.getElementAtIndex(0);
16409 final double biasY = ba.getElementAtIndex(1);
16410 final double biasZ = ba.getElementAtIndex(2);
16411
16412 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16413 final double latitude = Math.toRadians(
16414 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16415 final double longitude = Math.toRadians(
16416 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16417 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16418 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16419 final NEDVelocity nedVelocity = new NEDVelocity();
16420 final ECEFPosition ecefPosition = new ECEFPosition();
16421 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16422 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16423 ecefPosition, ecefVelocity);
16424
16425 KnownPositionAccelerometerCalibrator calibrator =
16426 new KnownPositionAccelerometerCalibrator(ecefPosition, bias);
16427
16428
16429 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16430 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16431 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16432 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16433 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16434 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16435 final Acceleration bx2 = new Acceleration(0.0,
16436 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16437 calibrator.getInitialBiasXAsAcceleration(bx2);
16438 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16439 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16440 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16441 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16442 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16443 final Acceleration by2 = new Acceleration(0.0,
16444 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16445 calibrator.getInitialBiasYAsAcceleration(by2);
16446 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16447 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16448 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16449 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16450 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16451 final Acceleration bz2 = new Acceleration(0.0,
16452 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16453 calibrator.getInitialBiasZAsAcceleration(bz2);
16454 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16455 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16456 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16457 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16458 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16459 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16460 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16461 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16462 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16463 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16464 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16465 final double[] bias1 = calibrator.getInitialBias();
16466 assertArrayEquals(bias1, bias, 0.0);
16467 final double[] bias2 = new double[3];
16468 calibrator.getInitialBias(bias2);
16469 assertArrayEquals(bias1, bias2, 0.0);
16470 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16471 assertEquals(b1, ba);
16472 final Matrix b2 = new Matrix(3, 1);
16473 calibrator.getInitialBiasAsMatrix(b2);
16474 assertEquals(b1, b2);
16475 final Matrix ma1 = calibrator.getInitialMa();
16476 assertEquals(ma1, new Matrix(3, 3));
16477 final Matrix ma2 = new Matrix(3, 3);
16478 calibrator.getInitialMa(ma2);
16479 assertEquals(ma1, ma2);
16480 assertSame(calibrator.getEcefPosition(), ecefPosition);
16481 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16482 final NEDPosition nedPosition1 = new NEDPosition();
16483 assertTrue(calibrator.getNedPosition(nedPosition1));
16484 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16485 assertNull(calibrator.getMeasurements());
16486 assertFalse(calibrator.isCommonAxisUsed());
16487 assertNull(calibrator.getListener());
16488 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16489 assertFalse(calibrator.isReady());
16490 assertFalse(calibrator.isRunning());
16491 assertNull(calibrator.getEstimatedBiases());
16492 assertFalse(calibrator.getEstimatedBiases(null));
16493 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16494 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16495 assertNull(calibrator.getEstimatedBiasFx());
16496 assertNull(calibrator.getEstimatedBiasFy());
16497 assertNull(calibrator.getEstimatedBiasFz());
16498 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16499 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16500 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16501 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16502 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16503 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16504 assertNull(calibrator.getEstimatedMa());
16505 assertNull(calibrator.getEstimatedSx());
16506 assertNull(calibrator.getEstimatedSy());
16507 assertNull(calibrator.getEstimatedSz());
16508 assertNull(calibrator.getEstimatedMxy());
16509 assertNull(calibrator.getEstimatedMxz());
16510 assertNull(calibrator.getEstimatedMyx());
16511 assertNull(calibrator.getEstimatedMyz());
16512 assertNull(calibrator.getEstimatedMzx());
16513 assertNull(calibrator.getEstimatedMzy());
16514 assertNull(calibrator.getEstimatedCovariance());
16515 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16516 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16517 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16518 assertNotNull(calibrator.getGroundTruthGravityNorm());
16519 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16520 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16521 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16522 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16523 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16524 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16525 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16526
16527
16528 calibrator = null;
16529 try {
16530 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16531 new double[1]);
16532 fail("IllegalArgumentException expected but not thrown");
16533 } catch (final IllegalArgumentException ignore) {
16534 }
16535 assertNull(calibrator);
16536 }
16537
16538 @Test
16539 public void testConstructor134() throws WrongSizeException {
16540 final Matrix ba = generateBa();
16541 final double[] bias = ba.getBuffer();
16542 final double biasX = ba.getElementAtIndex(0);
16543 final double biasY = ba.getElementAtIndex(1);
16544 final double biasZ = ba.getElementAtIndex(2);
16545
16546 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16547 final double latitude = Math.toRadians(
16548 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16549 final double longitude = Math.toRadians(
16550 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16551 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16552 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16553 final NEDVelocity nedVelocity = new NEDVelocity();
16554 final ECEFPosition ecefPosition = new ECEFPosition();
16555 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16556 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16557 ecefPosition, ecefVelocity);
16558
16559 KnownPositionAccelerometerCalibrator calibrator =
16560 new KnownPositionAccelerometerCalibrator(ecefPosition, bias,
16561 this);
16562
16563
16564 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16565 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16566 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16567 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16568 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16569 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16570 final Acceleration bx2 = new Acceleration(0.0,
16571 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16572 calibrator.getInitialBiasXAsAcceleration(bx2);
16573 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16574 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16575 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16576 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16577 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16578 final Acceleration by2 = new Acceleration(0.0,
16579 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16580 calibrator.getInitialBiasYAsAcceleration(by2);
16581 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16582 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16583 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16584 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16585 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16586 final Acceleration bz2 = new Acceleration(0.0,
16587 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16588 calibrator.getInitialBiasZAsAcceleration(bz2);
16589 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16590 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16591 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16592 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16593 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16594 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16595 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16596 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16597 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16598 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16599 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16600 final double[] bias1 = calibrator.getInitialBias();
16601 assertArrayEquals(bias1, bias, 0.0);
16602 final double[] bias2 = new double[3];
16603 calibrator.getInitialBias(bias2);
16604 assertArrayEquals(bias1, bias2, 0.0);
16605 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16606 assertEquals(b1, ba);
16607 final Matrix b2 = new Matrix(3, 1);
16608 calibrator.getInitialBiasAsMatrix(b2);
16609 assertEquals(b1, b2);
16610 final Matrix ma1 = calibrator.getInitialMa();
16611 assertEquals(ma1, new Matrix(3, 3));
16612 final Matrix ma2 = new Matrix(3, 3);
16613 calibrator.getInitialMa(ma2);
16614 assertEquals(ma1, ma2);
16615 assertSame(calibrator.getEcefPosition(), ecefPosition);
16616 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16617 final NEDPosition nedPosition1 = new NEDPosition();
16618 assertTrue(calibrator.getNedPosition(nedPosition1));
16619 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16620 assertNull(calibrator.getMeasurements());
16621 assertFalse(calibrator.isCommonAxisUsed());
16622 assertSame(calibrator.getListener(), this);
16623 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16624 assertFalse(calibrator.isReady());
16625 assertFalse(calibrator.isRunning());
16626 assertNull(calibrator.getEstimatedBiases());
16627 assertFalse(calibrator.getEstimatedBiases(null));
16628 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16629 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16630 assertNull(calibrator.getEstimatedBiasFx());
16631 assertNull(calibrator.getEstimatedBiasFy());
16632 assertNull(calibrator.getEstimatedBiasFz());
16633 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16634 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16635 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16636 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16637 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16638 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16639 assertNull(calibrator.getEstimatedMa());
16640 assertNull(calibrator.getEstimatedSx());
16641 assertNull(calibrator.getEstimatedSy());
16642 assertNull(calibrator.getEstimatedSz());
16643 assertNull(calibrator.getEstimatedMxy());
16644 assertNull(calibrator.getEstimatedMxz());
16645 assertNull(calibrator.getEstimatedMyx());
16646 assertNull(calibrator.getEstimatedMyz());
16647 assertNull(calibrator.getEstimatedMzx());
16648 assertNull(calibrator.getEstimatedMzy());
16649 assertNull(calibrator.getEstimatedCovariance());
16650 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16651 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16652 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16653 assertNotNull(calibrator.getGroundTruthGravityNorm());
16654 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16655 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16656 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16657 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16658 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16659 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16660 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16661
16662
16663 calibrator = null;
16664 try {
16665 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16666 new double[1], this);
16667 fail("IllegalArgumentException expected but not thrown");
16668 } catch (final IllegalArgumentException ignore) {
16669 }
16670 assertNull(calibrator);
16671 }
16672
16673 @Test
16674 public void testConstructor135() throws WrongSizeException {
16675 final Collection<StandardDeviationBodyKinematics> measurements =
16676 Collections.emptyList();
16677
16678 final Matrix ba = generateBa();
16679 final double[] bias = ba.getBuffer();
16680 final double biasX = ba.getElementAtIndex(0);
16681 final double biasY = ba.getElementAtIndex(1);
16682 final double biasZ = ba.getElementAtIndex(2);
16683
16684 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16685 final double latitude = Math.toRadians(
16686 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16687 final double longitude = Math.toRadians(
16688 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16689 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16690 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16691 final NEDVelocity nedVelocity = new NEDVelocity();
16692 final ECEFPosition ecefPosition = new ECEFPosition();
16693 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16694 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16695 ecefPosition, ecefVelocity);
16696
16697 KnownPositionAccelerometerCalibrator calibrator =
16698 new KnownPositionAccelerometerCalibrator(ecefPosition,
16699 measurements, bias);
16700
16701
16702 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16703 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16704 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16705 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16706 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16707 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16708 final Acceleration bx2 = new Acceleration(0.0,
16709 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16710 calibrator.getInitialBiasXAsAcceleration(bx2);
16711 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16712 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16713 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16714 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16715 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16716 final Acceleration by2 = new Acceleration(0.0,
16717 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16718 calibrator.getInitialBiasYAsAcceleration(by2);
16719 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16720 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16721 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16722 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16723 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16724 final Acceleration bz2 = new Acceleration(0.0,
16725 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16726 calibrator.getInitialBiasZAsAcceleration(bz2);
16727 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16728 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16729 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16730 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16731 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16732 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16733 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16734 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16735 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16736 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16737 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16738 final double[] bias1 = calibrator.getInitialBias();
16739 assertArrayEquals(bias1, bias, 0.0);
16740 final double[] bias2 = new double[3];
16741 calibrator.getInitialBias(bias2);
16742 assertArrayEquals(bias1, bias2, 0.0);
16743 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16744 assertEquals(b1, ba);
16745 final Matrix b2 = new Matrix(3, 1);
16746 calibrator.getInitialBiasAsMatrix(b2);
16747 assertEquals(b1, b2);
16748 final Matrix ma1 = calibrator.getInitialMa();
16749 assertEquals(ma1, new Matrix(3, 3));
16750 final Matrix ma2 = new Matrix(3, 3);
16751 calibrator.getInitialMa(ma2);
16752 assertEquals(ma1, ma2);
16753 assertSame(calibrator.getEcefPosition(), ecefPosition);
16754 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16755 final NEDPosition nedPosition1 = new NEDPosition();
16756 assertTrue(calibrator.getNedPosition(nedPosition1));
16757 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16758 assertSame(calibrator.getMeasurements(), measurements);
16759 assertFalse(calibrator.isCommonAxisUsed());
16760 assertNull(calibrator.getListener());
16761 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16762 assertFalse(calibrator.isReady());
16763 assertFalse(calibrator.isRunning());
16764 assertNull(calibrator.getEstimatedBiases());
16765 assertFalse(calibrator.getEstimatedBiases(null));
16766 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16767 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16768 assertNull(calibrator.getEstimatedBiasFx());
16769 assertNull(calibrator.getEstimatedBiasFy());
16770 assertNull(calibrator.getEstimatedBiasFz());
16771 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16772 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16773 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16774 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16775 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16776 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16777 assertNull(calibrator.getEstimatedMa());
16778 assertNull(calibrator.getEstimatedSx());
16779 assertNull(calibrator.getEstimatedSy());
16780 assertNull(calibrator.getEstimatedSz());
16781 assertNull(calibrator.getEstimatedMxy());
16782 assertNull(calibrator.getEstimatedMxz());
16783 assertNull(calibrator.getEstimatedMyx());
16784 assertNull(calibrator.getEstimatedMyz());
16785 assertNull(calibrator.getEstimatedMzx());
16786 assertNull(calibrator.getEstimatedMzy());
16787 assertNull(calibrator.getEstimatedCovariance());
16788 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16789 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16790 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16791 assertNotNull(calibrator.getGroundTruthGravityNorm());
16792 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16793 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16794 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16795 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16796 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16797 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16798 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16799
16800
16801 calibrator = null;
16802 try {
16803 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16804 measurements, new double[1]);
16805 fail("IllegalArgumentException expected but not thrown");
16806 } catch (final IllegalArgumentException ignore) {
16807 }
16808 assertNull(calibrator);
16809 }
16810
16811 @Test
16812 public void testConstructor136() throws WrongSizeException {
16813 final Collection<StandardDeviationBodyKinematics> measurements =
16814 Collections.emptyList();
16815
16816 final Matrix ba = generateBa();
16817 final double[] bias = ba.getBuffer();
16818 final double biasX = ba.getElementAtIndex(0);
16819 final double biasY = ba.getElementAtIndex(1);
16820 final double biasZ = ba.getElementAtIndex(2);
16821
16822 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16823 final double latitude = Math.toRadians(
16824 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16825 final double longitude = Math.toRadians(
16826 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16827 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16828 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16829 final NEDVelocity nedVelocity = new NEDVelocity();
16830 final ECEFPosition ecefPosition = new ECEFPosition();
16831 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16832 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16833 ecefPosition, ecefVelocity);
16834
16835 KnownPositionAccelerometerCalibrator calibrator =
16836 new KnownPositionAccelerometerCalibrator(ecefPosition,
16837 measurements, bias, this);
16838
16839
16840 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16841 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16842 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16843 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16844 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16845 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16846 final Acceleration bx2 = new Acceleration(0.0,
16847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16848 calibrator.getInitialBiasXAsAcceleration(bx2);
16849 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16850 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16851 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16852 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16853 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16854 final Acceleration by2 = new Acceleration(0.0,
16855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16856 calibrator.getInitialBiasYAsAcceleration(by2);
16857 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16858 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16859 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16860 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16861 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16862 final Acceleration bz2 = new Acceleration(0.0,
16863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16864 calibrator.getInitialBiasZAsAcceleration(bz2);
16865 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
16866 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16867 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
16868 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
16869 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
16870 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
16871 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
16872 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
16873 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
16874 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
16875 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
16876 final double[] bias1 = calibrator.getInitialBias();
16877 assertArrayEquals(bias1, bias, 0.0);
16878 final double[] bias2 = new double[3];
16879 calibrator.getInitialBias(bias2);
16880 assertArrayEquals(bias1, bias2, 0.0);
16881 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
16882 assertEquals(b1, ba);
16883 final Matrix b2 = new Matrix(3, 1);
16884 calibrator.getInitialBiasAsMatrix(b2);
16885 assertEquals(b1, b2);
16886 final Matrix ma1 = calibrator.getInitialMa();
16887 assertEquals(ma1, new Matrix(3, 3));
16888 final Matrix ma2 = new Matrix(3, 3);
16889 calibrator.getInitialMa(ma2);
16890 assertEquals(ma1, ma2);
16891 assertSame(calibrator.getEcefPosition(), ecefPosition);
16892 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
16893 final NEDPosition nedPosition1 = new NEDPosition();
16894 assertTrue(calibrator.getNedPosition(nedPosition1));
16895 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
16896 assertSame(calibrator.getMeasurements(), measurements);
16897 assertFalse(calibrator.isCommonAxisUsed());
16898 assertSame(calibrator.getListener(), this);
16899 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
16900 assertFalse(calibrator.isReady());
16901 assertFalse(calibrator.isRunning());
16902 assertNull(calibrator.getEstimatedBiases());
16903 assertFalse(calibrator.getEstimatedBiases(null));
16904 assertNull(calibrator.getEstimatedBiasesAsMatrix());
16905 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
16906 assertNull(calibrator.getEstimatedBiasFx());
16907 assertNull(calibrator.getEstimatedBiasFy());
16908 assertNull(calibrator.getEstimatedBiasFz());
16909 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
16910 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
16911 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
16912 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
16913 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
16914 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
16915 assertNull(calibrator.getEstimatedMa());
16916 assertNull(calibrator.getEstimatedSx());
16917 assertNull(calibrator.getEstimatedSy());
16918 assertNull(calibrator.getEstimatedSz());
16919 assertNull(calibrator.getEstimatedMxy());
16920 assertNull(calibrator.getEstimatedMxz());
16921 assertNull(calibrator.getEstimatedMyx());
16922 assertNull(calibrator.getEstimatedMyz());
16923 assertNull(calibrator.getEstimatedMzx());
16924 assertNull(calibrator.getEstimatedMzy());
16925 assertNull(calibrator.getEstimatedCovariance());
16926 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
16927 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
16928 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
16929 assertNotNull(calibrator.getGroundTruthGravityNorm());
16930 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
16931 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
16932 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
16933 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
16934 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
16935 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
16936 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
16937
16938
16939 calibrator = null;
16940 try {
16941 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
16942 measurements, new double[1], this);
16943 fail("IllegalArgumentException expected but not thrown");
16944 } catch (final IllegalArgumentException ignore) {
16945 }
16946 assertNull(calibrator);
16947 }
16948
16949 @Test
16950 public void testConstructor137() throws WrongSizeException {
16951 final Matrix ba = generateBa();
16952 final double[] bias = ba.getBuffer();
16953 final double biasX = ba.getElementAtIndex(0);
16954 final double biasY = ba.getElementAtIndex(1);
16955 final double biasZ = ba.getElementAtIndex(2);
16956
16957 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
16958 final double latitude = Math.toRadians(
16959 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
16960 final double longitude = Math.toRadians(
16961 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
16962 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
16963 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
16964 final NEDVelocity nedVelocity = new NEDVelocity();
16965 final ECEFPosition ecefPosition = new ECEFPosition();
16966 final ECEFVelocity ecefVelocity = new ECEFVelocity();
16967 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
16968 ecefPosition, ecefVelocity);
16969
16970 KnownPositionAccelerometerCalibrator calibrator =
16971 new KnownPositionAccelerometerCalibrator(ecefPosition,
16972 true, bias);
16973
16974
16975 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
16976 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
16977 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
16978 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
16979 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
16980 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16981 final Acceleration bx2 = new Acceleration(0.0,
16982 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16983 calibrator.getInitialBiasXAsAcceleration(bx2);
16984 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
16985 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16986 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
16987 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
16988 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16989 final Acceleration by2 = new Acceleration(0.0,
16990 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16991 calibrator.getInitialBiasYAsAcceleration(by2);
16992 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
16993 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16994 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
16995 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
16996 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
16997 final Acceleration bz2 = new Acceleration(0.0,
16998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
16999 calibrator.getInitialBiasZAsAcceleration(bz2);
17000 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17001 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17002 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17003 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17004 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17005 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17006 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17007 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17008 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17009 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17010 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17011 final double[] bias1 = calibrator.getInitialBias();
17012 assertArrayEquals(bias1, bias, 0.0);
17013 final double[] bias2 = new double[3];
17014 calibrator.getInitialBias(bias2);
17015 assertArrayEquals(bias1, bias2, 0.0);
17016 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17017 assertEquals(b1, ba);
17018 final Matrix b2 = new Matrix(3, 1);
17019 calibrator.getInitialBiasAsMatrix(b2);
17020 assertEquals(b1, b2);
17021 final Matrix ma1 = calibrator.getInitialMa();
17022 assertEquals(ma1, new Matrix(3, 3));
17023 final Matrix ma2 = new Matrix(3, 3);
17024 calibrator.getInitialMa(ma2);
17025 assertEquals(ma1, ma2);
17026 assertSame(calibrator.getEcefPosition(), ecefPosition);
17027 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17028 final NEDPosition nedPosition1 = new NEDPosition();
17029 assertTrue(calibrator.getNedPosition(nedPosition1));
17030 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17031 assertNull(calibrator.getMeasurements());
17032 assertTrue(calibrator.isCommonAxisUsed());
17033 assertNull(calibrator.getListener());
17034 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17035 assertFalse(calibrator.isReady());
17036 assertFalse(calibrator.isRunning());
17037 assertNull(calibrator.getEstimatedBiases());
17038 assertFalse(calibrator.getEstimatedBiases(null));
17039 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17040 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17041 assertNull(calibrator.getEstimatedBiasFx());
17042 assertNull(calibrator.getEstimatedBiasFy());
17043 assertNull(calibrator.getEstimatedBiasFz());
17044 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17045 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17046 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17047 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17048 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17049 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17050 assertNull(calibrator.getEstimatedMa());
17051 assertNull(calibrator.getEstimatedSx());
17052 assertNull(calibrator.getEstimatedSy());
17053 assertNull(calibrator.getEstimatedSz());
17054 assertNull(calibrator.getEstimatedMxy());
17055 assertNull(calibrator.getEstimatedMxz());
17056 assertNull(calibrator.getEstimatedMyx());
17057 assertNull(calibrator.getEstimatedMyz());
17058 assertNull(calibrator.getEstimatedMzx());
17059 assertNull(calibrator.getEstimatedMzy());
17060 assertNull(calibrator.getEstimatedCovariance());
17061 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17062 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17063 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17064 assertNotNull(calibrator.getGroundTruthGravityNorm());
17065 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17066 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17067 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17068 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17069 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17070 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17071 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17072
17073
17074 calibrator = null;
17075 try {
17076 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17077 true, new double[1]);
17078 fail("IllegalArgumentException expected but not thrown");
17079 } catch (final IllegalArgumentException ignore) {
17080 }
17081 assertNull(calibrator);
17082 }
17083
17084 @Test
17085 public void testConstructor138() throws WrongSizeException {
17086 final Matrix ba = generateBa();
17087 final double[] bias = ba.getBuffer();
17088 final double biasX = ba.getElementAtIndex(0);
17089 final double biasY = ba.getElementAtIndex(1);
17090 final double biasZ = ba.getElementAtIndex(2);
17091
17092 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17093 final double latitude = Math.toRadians(
17094 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17095 final double longitude = Math.toRadians(
17096 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17097 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17098 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17099 final NEDVelocity nedVelocity = new NEDVelocity();
17100 final ECEFPosition ecefPosition = new ECEFPosition();
17101 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17102 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17103 ecefPosition, ecefVelocity);
17104
17105 KnownPositionAccelerometerCalibrator calibrator =
17106 new KnownPositionAccelerometerCalibrator(ecefPosition,
17107 true, bias, this);
17108
17109
17110 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17111 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17112 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17113 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17114 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17115 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17116 final Acceleration bx2 = new Acceleration(0.0,
17117 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17118 calibrator.getInitialBiasXAsAcceleration(bx2);
17119 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17120 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17121 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17122 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17123 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17124 final Acceleration by2 = new Acceleration(0.0,
17125 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17126 calibrator.getInitialBiasYAsAcceleration(by2);
17127 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17128 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17129 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17130 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17131 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17132 final Acceleration bz2 = new Acceleration(0.0,
17133 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17134 calibrator.getInitialBiasZAsAcceleration(bz2);
17135 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17136 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17137 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17138 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17139 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17140 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17141 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17142 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17143 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17144 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17145 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17146 final double[] bias1 = calibrator.getInitialBias();
17147 assertArrayEquals(bias1, bias, 0.0);
17148 final double[] bias2 = new double[3];
17149 calibrator.getInitialBias(bias2);
17150 assertArrayEquals(bias1, bias2, 0.0);
17151 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17152 assertEquals(b1, ba);
17153 final Matrix b2 = new Matrix(3, 1);
17154 calibrator.getInitialBiasAsMatrix(b2);
17155 assertEquals(b1, b2);
17156 final Matrix ma1 = calibrator.getInitialMa();
17157 assertEquals(ma1, new Matrix(3, 3));
17158 final Matrix ma2 = new Matrix(3, 3);
17159 calibrator.getInitialMa(ma2);
17160 assertEquals(ma1, ma2);
17161 assertSame(calibrator.getEcefPosition(), ecefPosition);
17162 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17163 final NEDPosition nedPosition1 = new NEDPosition();
17164 assertTrue(calibrator.getNedPosition(nedPosition1));
17165 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17166 assertNull(calibrator.getMeasurements());
17167 assertTrue(calibrator.isCommonAxisUsed());
17168 assertSame(calibrator.getListener(), this);
17169 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17170 assertFalse(calibrator.isReady());
17171 assertFalse(calibrator.isRunning());
17172 assertNull(calibrator.getEstimatedBiases());
17173 assertFalse(calibrator.getEstimatedBiases(null));
17174 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17175 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17176 assertNull(calibrator.getEstimatedBiasFx());
17177 assertNull(calibrator.getEstimatedBiasFy());
17178 assertNull(calibrator.getEstimatedBiasFz());
17179 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17180 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17181 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17182 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17183 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17184 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17185 assertNull(calibrator.getEstimatedMa());
17186 assertNull(calibrator.getEstimatedSx());
17187 assertNull(calibrator.getEstimatedSy());
17188 assertNull(calibrator.getEstimatedSz());
17189 assertNull(calibrator.getEstimatedMxy());
17190 assertNull(calibrator.getEstimatedMxz());
17191 assertNull(calibrator.getEstimatedMyx());
17192 assertNull(calibrator.getEstimatedMyz());
17193 assertNull(calibrator.getEstimatedMzx());
17194 assertNull(calibrator.getEstimatedMzy());
17195 assertNull(calibrator.getEstimatedCovariance());
17196 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17197 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17198 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17199 assertNotNull(calibrator.getGroundTruthGravityNorm());
17200 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17201 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17202 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17203 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17204 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17205 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17206 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17207
17208
17209 calibrator = null;
17210 try {
17211 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17212 true, new double[1], this);
17213 fail("IllegalArgumentException expected but not thrown");
17214 } catch (final IllegalArgumentException ignore) {
17215 }
17216 assertNull(calibrator);
17217 }
17218
17219 @Test
17220 public void testConstructor139() throws WrongSizeException {
17221 final Collection<StandardDeviationBodyKinematics> measurements =
17222 Collections.emptyList();
17223
17224 final Matrix ba = generateBa();
17225 final double[] bias = ba.getBuffer();
17226 final double biasX = ba.getElementAtIndex(0);
17227 final double biasY = ba.getElementAtIndex(1);
17228 final double biasZ = ba.getElementAtIndex(2);
17229
17230 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17231 final double latitude = Math.toRadians(
17232 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17233 final double longitude = Math.toRadians(
17234 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17235 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17236 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17237 final NEDVelocity nedVelocity = new NEDVelocity();
17238 final ECEFPosition ecefPosition = new ECEFPosition();
17239 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17240 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17241 ecefPosition, ecefVelocity);
17242
17243 KnownPositionAccelerometerCalibrator calibrator =
17244 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17245 true, bias);
17246
17247
17248 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17249 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17250 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17251 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17252 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17253 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17254 final Acceleration bx2 = new Acceleration(0.0,
17255 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17256 calibrator.getInitialBiasXAsAcceleration(bx2);
17257 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17258 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17259 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17260 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17261 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17262 final Acceleration by2 = new Acceleration(0.0,
17263 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17264 calibrator.getInitialBiasYAsAcceleration(by2);
17265 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17266 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17267 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17268 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17269 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17270 final Acceleration bz2 = new Acceleration(0.0,
17271 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17272 calibrator.getInitialBiasZAsAcceleration(bz2);
17273 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17274 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17275 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17276 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17277 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17278 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17279 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17280 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17281 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17282 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17283 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17284 final double[] bias1 = calibrator.getInitialBias();
17285 assertArrayEquals(bias1, bias, 0.0);
17286 final double[] bias2 = new double[3];
17287 calibrator.getInitialBias(bias2);
17288 assertArrayEquals(bias1, bias2, 0.0);
17289 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17290 assertEquals(b1, ba);
17291 final Matrix b2 = new Matrix(3, 1);
17292 calibrator.getInitialBiasAsMatrix(b2);
17293 assertEquals(b1, b2);
17294 final Matrix ma1 = calibrator.getInitialMa();
17295 assertEquals(ma1, new Matrix(3, 3));
17296 final Matrix ma2 = new Matrix(3, 3);
17297 calibrator.getInitialMa(ma2);
17298 assertEquals(ma1, ma2);
17299 assertSame(calibrator.getEcefPosition(), ecefPosition);
17300 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17301 final NEDPosition nedPosition1 = new NEDPosition();
17302 assertTrue(calibrator.getNedPosition(nedPosition1));
17303 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17304 assertSame(calibrator.getMeasurements(), measurements);
17305 assertTrue(calibrator.isCommonAxisUsed());
17306 assertNull(calibrator.getListener());
17307 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17308 assertFalse(calibrator.isReady());
17309 assertFalse(calibrator.isRunning());
17310 assertNull(calibrator.getEstimatedBiases());
17311 assertFalse(calibrator.getEstimatedBiases(null));
17312 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17313 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17314 assertNull(calibrator.getEstimatedBiasFx());
17315 assertNull(calibrator.getEstimatedBiasFy());
17316 assertNull(calibrator.getEstimatedBiasFz());
17317 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17318 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17319 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17320 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17321 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17322 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17323 assertNull(calibrator.getEstimatedMa());
17324 assertNull(calibrator.getEstimatedSx());
17325 assertNull(calibrator.getEstimatedSy());
17326 assertNull(calibrator.getEstimatedSz());
17327 assertNull(calibrator.getEstimatedMxy());
17328 assertNull(calibrator.getEstimatedMxz());
17329 assertNull(calibrator.getEstimatedMyx());
17330 assertNull(calibrator.getEstimatedMyz());
17331 assertNull(calibrator.getEstimatedMzx());
17332 assertNull(calibrator.getEstimatedMzy());
17333 assertNull(calibrator.getEstimatedCovariance());
17334 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17335 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17336 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17337 assertNotNull(calibrator.getGroundTruthGravityNorm());
17338 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17339 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17340 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17341 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17342 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17343 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17344 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17345
17346
17347 calibrator = null;
17348 try {
17349 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17350 measurements, true, new double[1]);
17351 fail("IllegalArgumentException expected but not thrown");
17352 } catch (final IllegalArgumentException ignore) {
17353 }
17354 assertNull(calibrator);
17355 }
17356
17357 @Test
17358 public void testConstructor140() throws WrongSizeException {
17359 final Collection<StandardDeviationBodyKinematics> measurements =
17360 Collections.emptyList();
17361
17362 final Matrix ba = generateBa();
17363 final double[] bias = ba.getBuffer();
17364 final double biasX = ba.getElementAtIndex(0);
17365 final double biasY = ba.getElementAtIndex(1);
17366 final double biasZ = ba.getElementAtIndex(2);
17367
17368 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17369 final double latitude = Math.toRadians(
17370 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17371 final double longitude = Math.toRadians(
17372 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17373 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17374 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17375 final NEDVelocity nedVelocity = new NEDVelocity();
17376 final ECEFPosition ecefPosition = new ECEFPosition();
17377 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17378 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17379 ecefPosition, ecefVelocity);
17380
17381 KnownPositionAccelerometerCalibrator calibrator =
17382 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17383 true, bias, this);
17384
17385
17386 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17387 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17388 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17389 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17390 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17391 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17392 final Acceleration bx2 = new Acceleration(0.0,
17393 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17394 calibrator.getInitialBiasXAsAcceleration(bx2);
17395 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17396 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17397 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17398 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17399 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17400 final Acceleration by2 = new Acceleration(0.0,
17401 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17402 calibrator.getInitialBiasYAsAcceleration(by2);
17403 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17404 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17405 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17406 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17407 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17408 final Acceleration bz2 = new Acceleration(0.0,
17409 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17410 calibrator.getInitialBiasZAsAcceleration(bz2);
17411 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17412 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17413 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17414 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17415 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17416 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17417 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17418 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17419 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17420 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17421 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17422 final double[] bias1 = calibrator.getInitialBias();
17423 assertArrayEquals(bias1, bias, 0.0);
17424 final double[] bias2 = new double[3];
17425 calibrator.getInitialBias(bias2);
17426 assertArrayEquals(bias1, bias2, 0.0);
17427 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17428 assertEquals(b1, ba);
17429 final Matrix b2 = new Matrix(3, 1);
17430 calibrator.getInitialBiasAsMatrix(b2);
17431 assertEquals(b1, b2);
17432 final Matrix ma1 = calibrator.getInitialMa();
17433 assertEquals(ma1, new Matrix(3, 3));
17434 final Matrix ma2 = new Matrix(3, 3);
17435 calibrator.getInitialMa(ma2);
17436 assertEquals(ma1, ma2);
17437 assertSame(calibrator.getEcefPosition(), ecefPosition);
17438 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17439 final NEDPosition nedPosition1 = new NEDPosition();
17440 assertTrue(calibrator.getNedPosition(nedPosition1));
17441 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17442 assertSame(calibrator.getMeasurements(), measurements);
17443 assertTrue(calibrator.isCommonAxisUsed());
17444 assertSame(calibrator.getListener(), this);
17445 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
17446 assertFalse(calibrator.isReady());
17447 assertFalse(calibrator.isRunning());
17448 assertNull(calibrator.getEstimatedBiases());
17449 assertFalse(calibrator.getEstimatedBiases(null));
17450 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17451 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17452 assertNull(calibrator.getEstimatedBiasFx());
17453 assertNull(calibrator.getEstimatedBiasFy());
17454 assertNull(calibrator.getEstimatedBiasFz());
17455 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17456 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17457 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17458 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17459 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17460 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17461 assertNull(calibrator.getEstimatedMa());
17462 assertNull(calibrator.getEstimatedSx());
17463 assertNull(calibrator.getEstimatedSy());
17464 assertNull(calibrator.getEstimatedSz());
17465 assertNull(calibrator.getEstimatedMxy());
17466 assertNull(calibrator.getEstimatedMxz());
17467 assertNull(calibrator.getEstimatedMyx());
17468 assertNull(calibrator.getEstimatedMyz());
17469 assertNull(calibrator.getEstimatedMzx());
17470 assertNull(calibrator.getEstimatedMzy());
17471 assertNull(calibrator.getEstimatedCovariance());
17472 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17473 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17474 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17475 assertNotNull(calibrator.getGroundTruthGravityNorm());
17476 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17477 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17478 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17479 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17480 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17481 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17482 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17483
17484
17485 calibrator = null;
17486 try {
17487 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17488 measurements, true, new double[1], this);
17489 fail("IllegalArgumentException expected but not thrown");
17490 } catch (final IllegalArgumentException ignore) {
17491 }
17492 assertNull(calibrator);
17493 }
17494
17495 @Test
17496 public void testConstructor141() throws WrongSizeException {
17497 final Matrix ba = generateBa();
17498 final double[] bias = ba.getBuffer();
17499 final double biasX = ba.getElementAtIndex(0);
17500 final double biasY = ba.getElementAtIndex(1);
17501 final double biasZ = ba.getElementAtIndex(2);
17502
17503 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17504 final double latitude = Math.toRadians(
17505 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17506 final double longitude = Math.toRadians(
17507 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17508 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17509 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17510 final NEDVelocity nedVelocity = new NEDVelocity();
17511 final ECEFPosition ecefPosition = new ECEFPosition();
17512 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17513 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17514 ecefPosition, ecefVelocity);
17515
17516 KnownPositionAccelerometerCalibrator calibrator =
17517 new KnownPositionAccelerometerCalibrator(ecefPosition, ba);
17518
17519
17520 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17521 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17522 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17523 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17524 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17525 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17526 final Acceleration bx2 = new Acceleration(0.0,
17527 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17528 calibrator.getInitialBiasXAsAcceleration(bx2);
17529 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17530 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17531 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17532 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17533 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17534 final Acceleration by2 = new Acceleration(0.0,
17535 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17536 calibrator.getInitialBiasYAsAcceleration(by2);
17537 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17538 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17539 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17540 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17541 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17542 final Acceleration bz2 = new Acceleration(0.0,
17543 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17544 calibrator.getInitialBiasZAsAcceleration(bz2);
17545 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17546 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17547 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17548 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17549 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17550 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17551 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17552 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17553 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17554 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17555 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17556 final double[] bias1 = calibrator.getInitialBias();
17557 assertArrayEquals(bias1, bias, 0.0);
17558 final double[] bias2 = new double[3];
17559 calibrator.getInitialBias(bias2);
17560 assertArrayEquals(bias1, bias2, 0.0);
17561 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17562 assertEquals(b1, ba);
17563 final Matrix b2 = new Matrix(3, 1);
17564 calibrator.getInitialBiasAsMatrix(b2);
17565 assertEquals(b1, b2);
17566 final Matrix ma1 = calibrator.getInitialMa();
17567 assertEquals(ma1, new Matrix(3, 3));
17568 final Matrix ma2 = new Matrix(3, 3);
17569 calibrator.getInitialMa(ma2);
17570 assertEquals(ma1, ma2);
17571 assertSame(calibrator.getEcefPosition(), ecefPosition);
17572 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17573 final NEDPosition nedPosition1 = new NEDPosition();
17574 assertTrue(calibrator.getNedPosition(nedPosition1));
17575 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17576 assertNull(calibrator.getMeasurements());
17577 assertFalse(calibrator.isCommonAxisUsed());
17578 assertNull(calibrator.getListener());
17579 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17580 assertFalse(calibrator.isReady());
17581 assertFalse(calibrator.isRunning());
17582 assertNull(calibrator.getEstimatedBiases());
17583 assertFalse(calibrator.getEstimatedBiases(null));
17584 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17585 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17586 assertNull(calibrator.getEstimatedBiasFx());
17587 assertNull(calibrator.getEstimatedBiasFy());
17588 assertNull(calibrator.getEstimatedBiasFz());
17589 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17590 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17591 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17592 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17593 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17594 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17595 assertNull(calibrator.getEstimatedMa());
17596 assertNull(calibrator.getEstimatedSx());
17597 assertNull(calibrator.getEstimatedSy());
17598 assertNull(calibrator.getEstimatedSz());
17599 assertNull(calibrator.getEstimatedMxy());
17600 assertNull(calibrator.getEstimatedMxz());
17601 assertNull(calibrator.getEstimatedMyx());
17602 assertNull(calibrator.getEstimatedMyz());
17603 assertNull(calibrator.getEstimatedMzx());
17604 assertNull(calibrator.getEstimatedMzy());
17605 assertNull(calibrator.getEstimatedCovariance());
17606 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17607 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17608 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17609 assertNotNull(calibrator.getGroundTruthGravityNorm());
17610 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17611 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17612 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17613 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17614 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17615 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17616 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17617
17618
17619 calibrator = null;
17620 try {
17621 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17622 new Matrix(1, 1));
17623 fail("IllegalArgumentException expected but not thrown");
17624 } catch (final IllegalArgumentException ignore) {
17625 }
17626 try {
17627 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17628 new Matrix(1, 3));
17629 fail("IllegalArgumentException expected but not thrown");
17630 } catch (final IllegalArgumentException ignore) {
17631 }
17632 assertNull(calibrator);
17633 }
17634
17635 @Test
17636 public void testConstructor142() throws WrongSizeException {
17637 final Matrix ba = generateBa();
17638 final double[] bias = ba.getBuffer();
17639 final double biasX = ba.getElementAtIndex(0);
17640 final double biasY = ba.getElementAtIndex(1);
17641 final double biasZ = ba.getElementAtIndex(2);
17642
17643 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17644 final double latitude = Math.toRadians(
17645 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17646 final double longitude = Math.toRadians(
17647 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17648 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17649 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17650 final NEDVelocity nedVelocity = new NEDVelocity();
17651 final ECEFPosition ecefPosition = new ECEFPosition();
17652 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17653 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17654 ecefPosition, ecefVelocity);
17655
17656 KnownPositionAccelerometerCalibrator calibrator =
17657 new KnownPositionAccelerometerCalibrator(ecefPosition, ba,
17658 this);
17659
17660
17661 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17662 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17663 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17664 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17665 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17666 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17667 final Acceleration bx2 = new Acceleration(0.0,
17668 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17669 calibrator.getInitialBiasXAsAcceleration(bx2);
17670 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17671 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17672 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17673 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17674 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17675 final Acceleration by2 = new Acceleration(0.0,
17676 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17677 calibrator.getInitialBiasYAsAcceleration(by2);
17678 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17679 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17680 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17681 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17682 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17683 final Acceleration bz2 = new Acceleration(0.0,
17684 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17685 calibrator.getInitialBiasZAsAcceleration(bz2);
17686 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17687 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17688 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17689 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17690 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17691 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17692 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17693 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17694 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17695 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17696 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17697 final double[] bias1 = calibrator.getInitialBias();
17698 assertArrayEquals(bias1, bias, 0.0);
17699 final double[] bias2 = new double[3];
17700 calibrator.getInitialBias(bias2);
17701 assertArrayEquals(bias1, bias2, 0.0);
17702 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17703 assertEquals(b1, ba);
17704 final Matrix b2 = new Matrix(3, 1);
17705 calibrator.getInitialBiasAsMatrix(b2);
17706 assertEquals(b1, b2);
17707 final Matrix ma1 = calibrator.getInitialMa();
17708 assertEquals(ma1, new Matrix(3, 3));
17709 final Matrix ma2 = new Matrix(3, 3);
17710 calibrator.getInitialMa(ma2);
17711 assertEquals(ma1, ma2);
17712 assertSame(calibrator.getEcefPosition(), ecefPosition);
17713 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17714 final NEDPosition nedPosition1 = new NEDPosition();
17715 assertTrue(calibrator.getNedPosition(nedPosition1));
17716 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17717 assertNull(calibrator.getMeasurements());
17718 assertFalse(calibrator.isCommonAxisUsed());
17719 assertSame(calibrator.getListener(), this);
17720 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17721 assertFalse(calibrator.isReady());
17722 assertFalse(calibrator.isRunning());
17723 assertNull(calibrator.getEstimatedBiases());
17724 assertFalse(calibrator.getEstimatedBiases(null));
17725 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17726 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17727 assertNull(calibrator.getEstimatedBiasFx());
17728 assertNull(calibrator.getEstimatedBiasFy());
17729 assertNull(calibrator.getEstimatedBiasFz());
17730 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17731 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17732 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17733 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17734 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17735 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17736 assertNull(calibrator.getEstimatedMa());
17737 assertNull(calibrator.getEstimatedSx());
17738 assertNull(calibrator.getEstimatedSy());
17739 assertNull(calibrator.getEstimatedSz());
17740 assertNull(calibrator.getEstimatedMxy());
17741 assertNull(calibrator.getEstimatedMxz());
17742 assertNull(calibrator.getEstimatedMyx());
17743 assertNull(calibrator.getEstimatedMyz());
17744 assertNull(calibrator.getEstimatedMzx());
17745 assertNull(calibrator.getEstimatedMzy());
17746 assertNull(calibrator.getEstimatedCovariance());
17747 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17748 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17749 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17750 assertNotNull(calibrator.getGroundTruthGravityNorm());
17751 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17752 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17753 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17754 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17755 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17756 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17757 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17758
17759
17760 calibrator = null;
17761 try {
17762 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17763 new Matrix(1, 1), this);
17764 fail("IllegalArgumentException expected but not thrown");
17765 } catch (final IllegalArgumentException ignore) {
17766 }
17767 try {
17768 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17769 new Matrix(1, 3), this);
17770 fail("IllegalArgumentException expected but not thrown");
17771 } catch (final IllegalArgumentException ignore) {
17772 }
17773 assertNull(calibrator);
17774 }
17775
17776 @Test
17777 public void testConstructor143() throws WrongSizeException {
17778 final Collection<StandardDeviationBodyKinematics> measurements =
17779 Collections.emptyList();
17780
17781 final Matrix ba = generateBa();
17782 final double[] bias = ba.getBuffer();
17783 final double biasX = ba.getElementAtIndex(0);
17784 final double biasY = ba.getElementAtIndex(1);
17785 final double biasZ = ba.getElementAtIndex(2);
17786
17787 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17788 final double latitude = Math.toRadians(
17789 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17790 final double longitude = Math.toRadians(
17791 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17792 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17793 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17794 final NEDVelocity nedVelocity = new NEDVelocity();
17795 final ECEFPosition ecefPosition = new ECEFPosition();
17796 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17797 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17798 ecefPosition, ecefVelocity);
17799
17800 KnownPositionAccelerometerCalibrator calibrator =
17801 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17802 ba);
17803
17804
17805 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17806 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17807 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17808 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17809 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17810 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17811 final Acceleration bx2 = new Acceleration(0.0,
17812 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17813 calibrator.getInitialBiasXAsAcceleration(bx2);
17814 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17815 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17816 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17817 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17818 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17819 final Acceleration by2 = new Acceleration(0.0,
17820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17821 calibrator.getInitialBiasYAsAcceleration(by2);
17822 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17823 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17824 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17825 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17826 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17827 final Acceleration bz2 = new Acceleration(0.0,
17828 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17829 calibrator.getInitialBiasZAsAcceleration(bz2);
17830 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17831 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17832 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17833 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17834 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17835 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17836 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17837 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17838 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17839 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17840 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17841 final double[] bias1 = calibrator.getInitialBias();
17842 assertArrayEquals(bias1, bias, 0.0);
17843 final double[] bias2 = new double[3];
17844 calibrator.getInitialBias(bias2);
17845 assertArrayEquals(bias1, bias2, 0.0);
17846 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17847 assertEquals(b1, ba);
17848 final Matrix b2 = new Matrix(3, 1);
17849 calibrator.getInitialBiasAsMatrix(b2);
17850 assertEquals(b1, b2);
17851 final Matrix ma1 = calibrator.getInitialMa();
17852 assertEquals(ma1, new Matrix(3, 3));
17853 final Matrix ma2 = new Matrix(3, 3);
17854 calibrator.getInitialMa(ma2);
17855 assertEquals(ma1, ma2);
17856 assertSame(calibrator.getEcefPosition(), ecefPosition);
17857 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
17858 final NEDPosition nedPosition1 = new NEDPosition();
17859 assertTrue(calibrator.getNedPosition(nedPosition1));
17860 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
17861 assertSame(calibrator.getMeasurements(), measurements);
17862 assertFalse(calibrator.isCommonAxisUsed());
17863 assertNull(calibrator.getListener());
17864 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
17865 assertFalse(calibrator.isReady());
17866 assertFalse(calibrator.isRunning());
17867 assertNull(calibrator.getEstimatedBiases());
17868 assertFalse(calibrator.getEstimatedBiases(null));
17869 assertNull(calibrator.getEstimatedBiasesAsMatrix());
17870 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
17871 assertNull(calibrator.getEstimatedBiasFx());
17872 assertNull(calibrator.getEstimatedBiasFy());
17873 assertNull(calibrator.getEstimatedBiasFz());
17874 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
17875 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
17876 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
17877 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
17878 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
17879 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
17880 assertNull(calibrator.getEstimatedMa());
17881 assertNull(calibrator.getEstimatedSx());
17882 assertNull(calibrator.getEstimatedSy());
17883 assertNull(calibrator.getEstimatedSz());
17884 assertNull(calibrator.getEstimatedMxy());
17885 assertNull(calibrator.getEstimatedMxz());
17886 assertNull(calibrator.getEstimatedMyx());
17887 assertNull(calibrator.getEstimatedMyz());
17888 assertNull(calibrator.getEstimatedMzx());
17889 assertNull(calibrator.getEstimatedMzy());
17890 assertNull(calibrator.getEstimatedCovariance());
17891 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
17892 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
17893 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
17894 assertNotNull(calibrator.getGroundTruthGravityNorm());
17895 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
17896 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
17897 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
17898 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
17899 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
17900 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
17901 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
17902
17903
17904 calibrator = null;
17905 try {
17906 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17907 measurements, new Matrix(1, 1));
17908 fail("IllegalArgumentException expected but not thrown");
17909 } catch (final IllegalArgumentException ignore) {
17910 }
17911 try {
17912 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
17913 measurements, new Matrix(1, 3));
17914 fail("IllegalArgumentException expected but not thrown");
17915 } catch (final IllegalArgumentException ignore) {
17916 }
17917 assertNull(calibrator);
17918 }
17919
17920 @Test
17921 public void testConstructor144() throws WrongSizeException {
17922 final Collection<StandardDeviationBodyKinematics> measurements =
17923 Collections.emptyList();
17924
17925 final Matrix ba = generateBa();
17926 final double[] bias = ba.getBuffer();
17927 final double biasX = ba.getElementAtIndex(0);
17928 final double biasY = ba.getElementAtIndex(1);
17929 final double biasZ = ba.getElementAtIndex(2);
17930
17931 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
17932 final double latitude = Math.toRadians(
17933 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
17934 final double longitude = Math.toRadians(
17935 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
17936 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
17937 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
17938 final NEDVelocity nedVelocity = new NEDVelocity();
17939 final ECEFPosition ecefPosition = new ECEFPosition();
17940 final ECEFVelocity ecefVelocity = new ECEFVelocity();
17941 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
17942 ecefPosition, ecefVelocity);
17943
17944 KnownPositionAccelerometerCalibrator calibrator =
17945 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
17946 ba, this);
17947
17948
17949 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
17950 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
17951 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
17952 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
17953 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
17954 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17955 final Acceleration bx2 = new Acceleration(0.0,
17956 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17957 calibrator.getInitialBiasXAsAcceleration(bx2);
17958 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
17959 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17960 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
17961 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
17962 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17963 final Acceleration by2 = new Acceleration(0.0,
17964 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17965 calibrator.getInitialBiasYAsAcceleration(by2);
17966 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
17967 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17968 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
17969 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
17970 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17971 final Acceleration bz2 = new Acceleration(0.0,
17972 AccelerationUnit.FEET_PER_SQUARED_SECOND);
17973 calibrator.getInitialBiasZAsAcceleration(bz2);
17974 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
17975 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
17976 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
17977 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
17978 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
17979 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
17980 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
17981 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
17982 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
17983 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
17984 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
17985 final double[] bias1 = calibrator.getInitialBias();
17986 assertArrayEquals(bias1, bias, 0.0);
17987 final double[] bias2 = new double[3];
17988 calibrator.getInitialBias(bias2);
17989 assertArrayEquals(bias1, bias2, 0.0);
17990 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
17991 assertEquals(b1, ba);
17992 final Matrix b2 = new Matrix(3, 1);
17993 calibrator.getInitialBiasAsMatrix(b2);
17994 assertEquals(b1, b2);
17995 final Matrix ma1 = calibrator.getInitialMa();
17996 assertEquals(ma1, new Matrix(3, 3));
17997 final Matrix ma2 = new Matrix(3, 3);
17998 calibrator.getInitialMa(ma2);
17999 assertEquals(ma1, ma2);
18000 assertSame(calibrator.getEcefPosition(), ecefPosition);
18001 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18002 final NEDPosition nedPosition1 = new NEDPosition();
18003 assertTrue(calibrator.getNedPosition(nedPosition1));
18004 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18005 assertSame(calibrator.getMeasurements(), measurements);
18006 assertFalse(calibrator.isCommonAxisUsed());
18007 assertSame(calibrator.getListener(), this);
18008 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18009 assertFalse(calibrator.isReady());
18010 assertFalse(calibrator.isRunning());
18011 assertNull(calibrator.getEstimatedBiases());
18012 assertFalse(calibrator.getEstimatedBiases(null));
18013 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18014 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18015 assertNull(calibrator.getEstimatedBiasFx());
18016 assertNull(calibrator.getEstimatedBiasFy());
18017 assertNull(calibrator.getEstimatedBiasFz());
18018 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18019 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18020 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18021 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18022 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18023 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18024 assertNull(calibrator.getEstimatedMa());
18025 assertNull(calibrator.getEstimatedSx());
18026 assertNull(calibrator.getEstimatedSy());
18027 assertNull(calibrator.getEstimatedSz());
18028 assertNull(calibrator.getEstimatedMxy());
18029 assertNull(calibrator.getEstimatedMxz());
18030 assertNull(calibrator.getEstimatedMyx());
18031 assertNull(calibrator.getEstimatedMyz());
18032 assertNull(calibrator.getEstimatedMzx());
18033 assertNull(calibrator.getEstimatedMzy());
18034 assertNull(calibrator.getEstimatedCovariance());
18035 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18036 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18037 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18038 assertNotNull(calibrator.getGroundTruthGravityNorm());
18039 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18040 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18041 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18042 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18043 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18044 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18045 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18046
18047
18048 calibrator = null;
18049 try {
18050 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18051 measurements, new Matrix(1, 1), this);
18052 fail("IllegalArgumentException expected but not thrown");
18053 } catch (final IllegalArgumentException ignore) {
18054 }
18055 try {
18056 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18057 measurements, new Matrix(1, 3), this);
18058 fail("IllegalArgumentException expected but not thrown");
18059 } catch (final IllegalArgumentException ignore) {
18060 }
18061 assertNull(calibrator);
18062 }
18063
18064 @Test
18065 public void testConstructor145() throws WrongSizeException {
18066 final Matrix ba = generateBa();
18067 final double[] bias = ba.getBuffer();
18068 final double biasX = ba.getElementAtIndex(0);
18069 final double biasY = ba.getElementAtIndex(1);
18070 final double biasZ = ba.getElementAtIndex(2);
18071
18072 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18073 final double latitude = Math.toRadians(
18074 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18075 final double longitude = Math.toRadians(
18076 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18077 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18078 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18079 final NEDVelocity nedVelocity = new NEDVelocity();
18080 final ECEFPosition ecefPosition = new ECEFPosition();
18081 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18082 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18083 ecefPosition, ecefVelocity);
18084
18085 KnownPositionAccelerometerCalibrator calibrator =
18086 new KnownPositionAccelerometerCalibrator(ecefPosition,
18087 true, ba);
18088
18089
18090 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18091 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18092 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18093 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18094 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18095 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18096 final Acceleration bx2 = new Acceleration(0.0,
18097 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18098 calibrator.getInitialBiasXAsAcceleration(bx2);
18099 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18100 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18101 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18102 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18103 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18104 final Acceleration by2 = new Acceleration(0.0,
18105 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18106 calibrator.getInitialBiasYAsAcceleration(by2);
18107 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18108 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18109 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18110 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18111 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18112 final Acceleration bz2 = new Acceleration(0.0,
18113 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18114 calibrator.getInitialBiasZAsAcceleration(bz2);
18115 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18116 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18117 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18118 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18119 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18120 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18121 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18122 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18123 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18124 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18125 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18126 final double[] bias1 = calibrator.getInitialBias();
18127 assertArrayEquals(bias1, bias, 0.0);
18128 final double[] bias2 = new double[3];
18129 calibrator.getInitialBias(bias2);
18130 assertArrayEquals(bias1, bias2, 0.0);
18131 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18132 assertEquals(b1, ba);
18133 final Matrix b2 = new Matrix(3, 1);
18134 calibrator.getInitialBiasAsMatrix(b2);
18135 assertEquals(b1, b2);
18136 final Matrix ma1 = calibrator.getInitialMa();
18137 assertEquals(ma1, new Matrix(3, 3));
18138 final Matrix ma2 = new Matrix(3, 3);
18139 calibrator.getInitialMa(ma2);
18140 assertEquals(ma1, ma2);
18141 assertSame(calibrator.getEcefPosition(), ecefPosition);
18142 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18143 final NEDPosition nedPosition1 = new NEDPosition();
18144 assertTrue(calibrator.getNedPosition(nedPosition1));
18145 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18146 assertNull(calibrator.getMeasurements());
18147 assertTrue(calibrator.isCommonAxisUsed());
18148 assertNull(calibrator.getListener());
18149 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18150 assertFalse(calibrator.isReady());
18151 assertFalse(calibrator.isRunning());
18152 assertNull(calibrator.getEstimatedBiases());
18153 assertFalse(calibrator.getEstimatedBiases(null));
18154 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18155 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18156 assertNull(calibrator.getEstimatedBiasFx());
18157 assertNull(calibrator.getEstimatedBiasFy());
18158 assertNull(calibrator.getEstimatedBiasFz());
18159 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18160 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18161 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18162 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18163 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18164 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18165 assertNull(calibrator.getEstimatedMa());
18166 assertNull(calibrator.getEstimatedSx());
18167 assertNull(calibrator.getEstimatedSy());
18168 assertNull(calibrator.getEstimatedSz());
18169 assertNull(calibrator.getEstimatedMxy());
18170 assertNull(calibrator.getEstimatedMxz());
18171 assertNull(calibrator.getEstimatedMyx());
18172 assertNull(calibrator.getEstimatedMyz());
18173 assertNull(calibrator.getEstimatedMzx());
18174 assertNull(calibrator.getEstimatedMzy());
18175 assertNull(calibrator.getEstimatedCovariance());
18176 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18177 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18178 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18179 assertNotNull(calibrator.getGroundTruthGravityNorm());
18180 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18181 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18182 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18183 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18184 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18185 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18186 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18187
18188
18189 calibrator = null;
18190 try {
18191 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18192 true, new Matrix(1, 1));
18193 fail("IllegalArgumentException expected but not thrown");
18194 } catch (final IllegalArgumentException ignore) {
18195 }
18196 try {
18197 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18198 true, new Matrix(1, 3));
18199 fail("IllegalArgumentException expected but not thrown");
18200 } catch (final IllegalArgumentException ignore) {
18201 }
18202 assertNull(calibrator);
18203 }
18204
18205 @Test
18206 public void testConstructor146() throws WrongSizeException {
18207 final Matrix ba = generateBa();
18208 final double[] bias = ba.getBuffer();
18209 final double biasX = ba.getElementAtIndex(0);
18210 final double biasY = ba.getElementAtIndex(1);
18211 final double biasZ = ba.getElementAtIndex(2);
18212
18213 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18214 final double latitude = Math.toRadians(
18215 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18216 final double longitude = Math.toRadians(
18217 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18218 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18219 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18220 final NEDVelocity nedVelocity = new NEDVelocity();
18221 final ECEFPosition ecefPosition = new ECEFPosition();
18222 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18223 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18224 ecefPosition, ecefVelocity);
18225
18226 KnownPositionAccelerometerCalibrator calibrator =
18227 new KnownPositionAccelerometerCalibrator(ecefPosition,
18228 true, ba, this);
18229
18230
18231 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18232 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18233 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18234 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18235 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18236 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18237 final Acceleration bx2 = new Acceleration(0.0,
18238 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18239 calibrator.getInitialBiasXAsAcceleration(bx2);
18240 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18241 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18242 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18243 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18244 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18245 final Acceleration by2 = new Acceleration(0.0,
18246 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18247 calibrator.getInitialBiasYAsAcceleration(by2);
18248 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18249 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18250 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18251 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18252 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18253 final Acceleration bz2 = new Acceleration(0.0,
18254 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18255 calibrator.getInitialBiasZAsAcceleration(bz2);
18256 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18257 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18258 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18259 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18260 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18261 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18262 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18263 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18264 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18265 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18266 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18267 final double[] bias1 = calibrator.getInitialBias();
18268 assertArrayEquals(bias1, bias, 0.0);
18269 final double[] bias2 = new double[3];
18270 calibrator.getInitialBias(bias2);
18271 assertArrayEquals(bias1, bias2, 0.0);
18272 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18273 assertEquals(b1, ba);
18274 final Matrix b2 = new Matrix(3, 1);
18275 calibrator.getInitialBiasAsMatrix(b2);
18276 assertEquals(b1, b2);
18277 final Matrix ma1 = calibrator.getInitialMa();
18278 assertEquals(ma1, new Matrix(3, 3));
18279 final Matrix ma2 = new Matrix(3, 3);
18280 calibrator.getInitialMa(ma2);
18281 assertEquals(ma1, ma2);
18282 assertSame(calibrator.getEcefPosition(), ecefPosition);
18283 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18284 final NEDPosition nedPosition1 = new NEDPosition();
18285 assertTrue(calibrator.getNedPosition(nedPosition1));
18286 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18287 assertNull(calibrator.getMeasurements());
18288 assertTrue(calibrator.isCommonAxisUsed());
18289 assertSame(calibrator.getListener(), this);
18290 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18291 assertFalse(calibrator.isReady());
18292 assertFalse(calibrator.isRunning());
18293 assertNull(calibrator.getEstimatedBiases());
18294 assertFalse(calibrator.getEstimatedBiases(null));
18295 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18296 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18297 assertNull(calibrator.getEstimatedBiasFx());
18298 assertNull(calibrator.getEstimatedBiasFy());
18299 assertNull(calibrator.getEstimatedBiasFz());
18300 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18301 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18302 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18303 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18304 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18305 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18306 assertNull(calibrator.getEstimatedMa());
18307 assertNull(calibrator.getEstimatedSx());
18308 assertNull(calibrator.getEstimatedSy());
18309 assertNull(calibrator.getEstimatedSz());
18310 assertNull(calibrator.getEstimatedMxy());
18311 assertNull(calibrator.getEstimatedMxz());
18312 assertNull(calibrator.getEstimatedMyx());
18313 assertNull(calibrator.getEstimatedMyz());
18314 assertNull(calibrator.getEstimatedMzx());
18315 assertNull(calibrator.getEstimatedMzy());
18316 assertNull(calibrator.getEstimatedCovariance());
18317 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18318 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18319 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18320 assertNotNull(calibrator.getGroundTruthGravityNorm());
18321 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18322 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18323 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18324 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18325 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18326 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18327 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18328
18329
18330 calibrator = null;
18331 try {
18332 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18333 true, new Matrix(1, 1),
18334 this);
18335 fail("IllegalArgumentException expected but not thrown");
18336 } catch (final IllegalArgumentException ignore) {
18337 }
18338 try {
18339 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18340 true, new Matrix(1, 3),
18341 this);
18342 fail("IllegalArgumentException expected but not thrown");
18343 } catch (final IllegalArgumentException ignore) {
18344 }
18345 assertNull(calibrator);
18346 }
18347
18348 @Test
18349 public void testConstructor147() throws WrongSizeException {
18350 final Collection<StandardDeviationBodyKinematics> measurements =
18351 Collections.emptyList();
18352
18353 final Matrix ba = generateBa();
18354 final double[] bias = ba.getBuffer();
18355 final double biasX = ba.getElementAtIndex(0);
18356 final double biasY = ba.getElementAtIndex(1);
18357 final double biasZ = ba.getElementAtIndex(2);
18358
18359 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18360 final double latitude = Math.toRadians(
18361 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18362 final double longitude = Math.toRadians(
18363 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18364 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18365 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18366 final NEDVelocity nedVelocity = new NEDVelocity();
18367 final ECEFPosition ecefPosition = new ECEFPosition();
18368 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18369 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18370 ecefPosition, ecefVelocity);
18371
18372 KnownPositionAccelerometerCalibrator calibrator =
18373 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18374 true, ba);
18375
18376
18377 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18378 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18379 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18380 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18381 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18382 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18383 final Acceleration bx2 = new Acceleration(0.0,
18384 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18385 calibrator.getInitialBiasXAsAcceleration(bx2);
18386 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18387 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18388 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18389 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18390 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18391 final Acceleration by2 = new Acceleration(0.0,
18392 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18393 calibrator.getInitialBiasYAsAcceleration(by2);
18394 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18395 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18396 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18397 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18398 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18399 final Acceleration bz2 = new Acceleration(0.0,
18400 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18401 calibrator.getInitialBiasZAsAcceleration(bz2);
18402 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18403 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18404 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18405 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18406 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18407 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18408 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18409 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18410 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18411 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18412 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18413 final double[] bias1 = calibrator.getInitialBias();
18414 assertArrayEquals(bias1, bias, 0.0);
18415 final double[] bias2 = new double[3];
18416 calibrator.getInitialBias(bias2);
18417 assertArrayEquals(bias1, bias2, 0.0);
18418 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18419 assertEquals(b1, ba);
18420 final Matrix b2 = new Matrix(3, 1);
18421 calibrator.getInitialBiasAsMatrix(b2);
18422 assertEquals(b1, b2);
18423 final Matrix ma1 = calibrator.getInitialMa();
18424 assertEquals(ma1, new Matrix(3, 3));
18425 final Matrix ma2 = new Matrix(3, 3);
18426 calibrator.getInitialMa(ma2);
18427 assertEquals(ma1, ma2);
18428 assertSame(calibrator.getEcefPosition(), ecefPosition);
18429 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18430 final NEDPosition nedPosition1 = new NEDPosition();
18431 assertTrue(calibrator.getNedPosition(nedPosition1));
18432 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18433 assertSame(calibrator.getMeasurements(), measurements);
18434 assertTrue(calibrator.isCommonAxisUsed());
18435 assertNull(calibrator.getListener());
18436 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18437 assertFalse(calibrator.isReady());
18438 assertFalse(calibrator.isRunning());
18439 assertNull(calibrator.getEstimatedBiases());
18440 assertFalse(calibrator.getEstimatedBiases(null));
18441 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18442 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18443 assertNull(calibrator.getEstimatedBiasFx());
18444 assertNull(calibrator.getEstimatedBiasFy());
18445 assertNull(calibrator.getEstimatedBiasFz());
18446 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18447 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18448 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18449 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18450 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18451 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18452 assertNull(calibrator.getEstimatedMa());
18453 assertNull(calibrator.getEstimatedSx());
18454 assertNull(calibrator.getEstimatedSy());
18455 assertNull(calibrator.getEstimatedSz());
18456 assertNull(calibrator.getEstimatedMxy());
18457 assertNull(calibrator.getEstimatedMxz());
18458 assertNull(calibrator.getEstimatedMyx());
18459 assertNull(calibrator.getEstimatedMyz());
18460 assertNull(calibrator.getEstimatedMzx());
18461 assertNull(calibrator.getEstimatedMzy());
18462 assertNull(calibrator.getEstimatedCovariance());
18463 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18464 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18465 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18466 assertNotNull(calibrator.getGroundTruthGravityNorm());
18467 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18468 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18469 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18470 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18471 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18472 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18473 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18474
18475
18476 calibrator = null;
18477 try {
18478 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18479 measurements, true,
18480 new Matrix(1, 1));
18481 fail("IllegalArgumentException expected but not thrown");
18482 } catch (final IllegalArgumentException ignore) {
18483 }
18484 try {
18485 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18486 measurements, true,
18487 new Matrix(1, 3));
18488 fail("IllegalArgumentException expected but not thrown");
18489 } catch (final IllegalArgumentException ignore) {
18490 }
18491 assertNull(calibrator);
18492 }
18493
18494 @Test
18495 public void testConstructor148() throws WrongSizeException {
18496 final Collection<StandardDeviationBodyKinematics> measurements =
18497 Collections.emptyList();
18498
18499 final Matrix ba = generateBa();
18500 final double[] bias = ba.getBuffer();
18501 final double biasX = ba.getElementAtIndex(0);
18502 final double biasY = ba.getElementAtIndex(1);
18503 final double biasZ = ba.getElementAtIndex(2);
18504
18505 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18506 final double latitude = Math.toRadians(
18507 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18508 final double longitude = Math.toRadians(
18509 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18510 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18511 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18512 final NEDVelocity nedVelocity = new NEDVelocity();
18513 final ECEFPosition ecefPosition = new ECEFPosition();
18514 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18515 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18516 ecefPosition, ecefVelocity);
18517
18518 KnownPositionAccelerometerCalibrator calibrator =
18519 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
18520 true, ba, this);
18521
18522
18523 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18524 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18525 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18526 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18527 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18528 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18529 final Acceleration bx2 = new Acceleration(0.0,
18530 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18531 calibrator.getInitialBiasXAsAcceleration(bx2);
18532 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18533 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18534 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18535 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18536 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18537 final Acceleration by2 = new Acceleration(0.0,
18538 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18539 calibrator.getInitialBiasYAsAcceleration(by2);
18540 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18541 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18542 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18543 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18544 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18545 final Acceleration bz2 = new Acceleration(0.0,
18546 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18547 calibrator.getInitialBiasZAsAcceleration(bz2);
18548 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18549 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18550 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
18551 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
18552 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
18553 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
18554 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
18555 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
18556 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
18557 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
18558 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
18559 final double[] bias1 = calibrator.getInitialBias();
18560 assertArrayEquals(bias1, bias, 0.0);
18561 final double[] bias2 = new double[3];
18562 calibrator.getInitialBias(bias2);
18563 assertArrayEquals(bias1, bias2, 0.0);
18564 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18565 assertEquals(b1, ba);
18566 final Matrix b2 = new Matrix(3, 1);
18567 calibrator.getInitialBiasAsMatrix(b2);
18568 assertEquals(b1, b2);
18569 final Matrix ma1 = calibrator.getInitialMa();
18570 assertEquals(ma1, new Matrix(3, 3));
18571 final Matrix ma2 = new Matrix(3, 3);
18572 calibrator.getInitialMa(ma2);
18573 assertEquals(ma1, ma2);
18574 assertSame(calibrator.getEcefPosition(), ecefPosition);
18575 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18576 final NEDPosition nedPosition1 = new NEDPosition();
18577 assertTrue(calibrator.getNedPosition(nedPosition1));
18578 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18579 assertSame(calibrator.getMeasurements(), measurements);
18580 assertTrue(calibrator.isCommonAxisUsed());
18581 assertSame(calibrator.getListener(), this);
18582 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
18583 assertFalse(calibrator.isReady());
18584 assertFalse(calibrator.isRunning());
18585 assertNull(calibrator.getEstimatedBiases());
18586 assertFalse(calibrator.getEstimatedBiases(null));
18587 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18588 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18589 assertNull(calibrator.getEstimatedBiasFx());
18590 assertNull(calibrator.getEstimatedBiasFy());
18591 assertNull(calibrator.getEstimatedBiasFz());
18592 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18593 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18594 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18595 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18596 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18597 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18598 assertNull(calibrator.getEstimatedMa());
18599 assertNull(calibrator.getEstimatedSx());
18600 assertNull(calibrator.getEstimatedSy());
18601 assertNull(calibrator.getEstimatedSz());
18602 assertNull(calibrator.getEstimatedMxy());
18603 assertNull(calibrator.getEstimatedMxz());
18604 assertNull(calibrator.getEstimatedMyx());
18605 assertNull(calibrator.getEstimatedMyz());
18606 assertNull(calibrator.getEstimatedMzx());
18607 assertNull(calibrator.getEstimatedMzy());
18608 assertNull(calibrator.getEstimatedCovariance());
18609 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18610 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18611 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18612 assertNotNull(calibrator.getGroundTruthGravityNorm());
18613 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18614 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18615 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18616 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18617 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18618 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18619 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18620
18621
18622 calibrator = null;
18623 try {
18624 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18625 measurements, true,
18626 new Matrix(1, 1), this);
18627 fail("IllegalArgumentException expected but not thrown");
18628 } catch (final IllegalArgumentException ignore) {
18629 }
18630 try {
18631 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18632 measurements, true,
18633 new Matrix(1, 3), this);
18634 fail("IllegalArgumentException expected but not thrown");
18635 } catch (final IllegalArgumentException ignore) {
18636 }
18637 assertNull(calibrator);
18638 }
18639
18640 @Test
18641 public void testConstructor149() throws WrongSizeException {
18642 final Matrix ba = generateBa();
18643 final double[] bias = ba.getBuffer();
18644 final double biasX = ba.getElementAtIndex(0);
18645 final double biasY = ba.getElementAtIndex(1);
18646 final double biasZ = ba.getElementAtIndex(2);
18647
18648 final Matrix ma = generateMaCommonAxis();
18649 final double sx = ma.getElementAt(0, 0);
18650 final double sy = ma.getElementAt(1, 1);
18651 final double sz = ma.getElementAt(2, 2);
18652 final double mxy = ma.getElementAt(0, 1);
18653 final double mxz = ma.getElementAt(0, 2);
18654 final double myx = ma.getElementAt(1, 0);
18655 final double myz = ma.getElementAt(1, 2);
18656 final double mzx = ma.getElementAt(2, 0);
18657 final double mzy = ma.getElementAt(2, 1);
18658
18659 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18660 final double latitude = Math.toRadians(
18661 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18662 final double longitude = Math.toRadians(
18663 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18664 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18665 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18666 final NEDVelocity nedVelocity = new NEDVelocity();
18667 final ECEFPosition ecefPosition = new ECEFPosition();
18668 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18669 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18670 ecefPosition, ecefVelocity);
18671
18672 KnownPositionAccelerometerCalibrator calibrator =
18673 new KnownPositionAccelerometerCalibrator(ecefPosition, ba, ma);
18674
18675
18676 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18677 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18678 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18679 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18680 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18681 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18682 final Acceleration bx2 = new Acceleration(0.0,
18683 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18684 calibrator.getInitialBiasXAsAcceleration(bx2);
18685 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18686 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18687 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18688 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18689 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18690 final Acceleration by2 = new Acceleration(0.0,
18691 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18692 calibrator.getInitialBiasYAsAcceleration(by2);
18693 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18694 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18695 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18696 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18697 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18698 final Acceleration bz2 = new Acceleration(0.0,
18699 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18700 calibrator.getInitialBiasZAsAcceleration(bz2);
18701 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18702 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18703 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18704 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18705 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18706 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18707 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18708 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18709 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18710 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18711 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18712 final double[] bias1 = calibrator.getInitialBias();
18713 assertArrayEquals(bias1, bias, 0.0);
18714 final double[] bias2 = new double[3];
18715 calibrator.getInitialBias(bias2);
18716 assertArrayEquals(bias1, bias2, 0.0);
18717 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18718 assertEquals(b1, ba);
18719 final Matrix b2 = new Matrix(3, 1);
18720 calibrator.getInitialBiasAsMatrix(b2);
18721 assertEquals(b1, b2);
18722 final Matrix ma1 = new Matrix(3, 3);
18723 ma1.setSubmatrix(0, 0,
18724 2, 2,
18725 new double[]{sx, myx, mzx,
18726 mxy, sy, mzy,
18727 mxz, myz, sz});
18728 assertEquals(calibrator.getInitialMa(), ma1);
18729 final Matrix ma2 = new Matrix(3, 3);
18730 calibrator.getInitialMa(ma2);
18731 assertEquals(ma1, ma2);
18732 assertSame(calibrator.getEcefPosition(), ecefPosition);
18733 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18734 final NEDPosition nedPosition1 = new NEDPosition();
18735 assertTrue(calibrator.getNedPosition(nedPosition1));
18736 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18737 assertNull(calibrator.getMeasurements());
18738 assertFalse(calibrator.isCommonAxisUsed());
18739 assertNull(calibrator.getListener());
18740 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18741 assertFalse(calibrator.isReady());
18742 assertFalse(calibrator.isRunning());
18743 assertNull(calibrator.getEstimatedBiases());
18744 assertFalse(calibrator.getEstimatedBiases(null));
18745 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18746 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18747 assertNull(calibrator.getEstimatedBiasFx());
18748 assertNull(calibrator.getEstimatedBiasFy());
18749 assertNull(calibrator.getEstimatedBiasFz());
18750 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18751 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18752 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18753 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18754 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18755 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18756 assertNull(calibrator.getEstimatedMa());
18757 assertNull(calibrator.getEstimatedSx());
18758 assertNull(calibrator.getEstimatedSy());
18759 assertNull(calibrator.getEstimatedSz());
18760 assertNull(calibrator.getEstimatedMxy());
18761 assertNull(calibrator.getEstimatedMxz());
18762 assertNull(calibrator.getEstimatedMyx());
18763 assertNull(calibrator.getEstimatedMyz());
18764 assertNull(calibrator.getEstimatedMzx());
18765 assertNull(calibrator.getEstimatedMzy());
18766 assertNull(calibrator.getEstimatedCovariance());
18767 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18768 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18769 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18770 assertNotNull(calibrator.getGroundTruthGravityNorm());
18771 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18772 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18773 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18774 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18775 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18776 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18777 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18778
18779
18780 calibrator = null;
18781 try {
18782 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18783 new Matrix(1, 1), ma);
18784 fail("IllegalArgumentException expected but not thrown");
18785 } catch (final IllegalArgumentException ignore) {
18786 }
18787 try {
18788 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18789 new Matrix(1, 3), ma);
18790 fail("IllegalArgumentException expected but not thrown");
18791 } catch (final IllegalArgumentException ignore) {
18792 }
18793 try {
18794 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18795 ba, new Matrix(1, 3));
18796 fail("IllegalArgumentException expected but not thrown");
18797 } catch (final IllegalArgumentException ignore) {
18798 }
18799 try {
18800 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18801 ba, new Matrix(3, 1));
18802 fail("IllegalArgumentException expected but not thrown");
18803 } catch (final IllegalArgumentException ignore) {
18804 }
18805 assertNull(calibrator);
18806 }
18807
18808 @Test
18809 public void testConstructor150() throws WrongSizeException {
18810 final Matrix ba = generateBa();
18811 final double[] bias = ba.getBuffer();
18812 final double biasX = ba.getElementAtIndex(0);
18813 final double biasY = ba.getElementAtIndex(1);
18814 final double biasZ = ba.getElementAtIndex(2);
18815
18816 final Matrix ma = generateMaCommonAxis();
18817 final double sx = ma.getElementAt(0, 0);
18818 final double sy = ma.getElementAt(1, 1);
18819 final double sz = ma.getElementAt(2, 2);
18820 final double mxy = ma.getElementAt(0, 1);
18821 final double mxz = ma.getElementAt(0, 2);
18822 final double myx = ma.getElementAt(1, 0);
18823 final double myz = ma.getElementAt(1, 2);
18824 final double mzx = ma.getElementAt(2, 0);
18825 final double mzy = ma.getElementAt(2, 1);
18826
18827 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
18828 final double latitude = Math.toRadians(
18829 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
18830 final double longitude = Math.toRadians(
18831 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
18832 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
18833 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
18834 final NEDVelocity nedVelocity = new NEDVelocity();
18835 final ECEFPosition ecefPosition = new ECEFPosition();
18836 final ECEFVelocity ecefVelocity = new ECEFVelocity();
18837 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
18838 ecefPosition, ecefVelocity);
18839
18840 KnownPositionAccelerometerCalibrator calibrator =
18841 new KnownPositionAccelerometerCalibrator(ecefPosition,
18842 ba, ma, this);
18843
18844
18845 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
18846 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
18847 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
18848 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
18849 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
18850 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18851 final Acceleration bx2 = new Acceleration(0.0,
18852 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18853 calibrator.getInitialBiasXAsAcceleration(bx2);
18854 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
18855 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18856 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
18857 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
18858 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18859 final Acceleration by2 = new Acceleration(0.0,
18860 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18861 calibrator.getInitialBiasYAsAcceleration(by2);
18862 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
18863 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18864 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
18865 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
18866 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18867 final Acceleration bz2 = new Acceleration(0.0,
18868 AccelerationUnit.FEET_PER_SQUARED_SECOND);
18869 calibrator.getInitialBiasZAsAcceleration(bz2);
18870 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
18871 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
18872 assertEquals(calibrator.getInitialSx(), sx, 0.0);
18873 assertEquals(calibrator.getInitialSy(), sy, 0.0);
18874 assertEquals(calibrator.getInitialSz(), sz, 0.0);
18875 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
18876 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
18877 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
18878 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
18879 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
18880 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
18881 final double[] bias1 = calibrator.getInitialBias();
18882 assertArrayEquals(bias1, bias, 0.0);
18883 final double[] bias2 = new double[3];
18884 calibrator.getInitialBias(bias2);
18885 assertArrayEquals(bias1, bias2, 0.0);
18886 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
18887 assertEquals(b1, ba);
18888 final Matrix b2 = new Matrix(3, 1);
18889 calibrator.getInitialBiasAsMatrix(b2);
18890 assertEquals(b1, b2);
18891 final Matrix ma1 = new Matrix(3, 3);
18892 ma1.setSubmatrix(0, 0,
18893 2, 2,
18894 new double[]{sx, myx, mzx,
18895 mxy, sy, mzy,
18896 mxz, myz, sz});
18897 assertEquals(calibrator.getInitialMa(), ma1);
18898 final Matrix ma2 = new Matrix(3, 3);
18899 calibrator.getInitialMa(ma2);
18900 assertEquals(ma1, ma2);
18901 assertSame(calibrator.getEcefPosition(), ecefPosition);
18902 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
18903 final NEDPosition nedPosition1 = new NEDPosition();
18904 assertTrue(calibrator.getNedPosition(nedPosition1));
18905 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
18906 assertNull(calibrator.getMeasurements());
18907 assertFalse(calibrator.isCommonAxisUsed());
18908 assertSame(calibrator.getListener(), this);
18909 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
18910 assertFalse(calibrator.isReady());
18911 assertFalse(calibrator.isRunning());
18912 assertNull(calibrator.getEstimatedBiases());
18913 assertFalse(calibrator.getEstimatedBiases(null));
18914 assertNull(calibrator.getEstimatedBiasesAsMatrix());
18915 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
18916 assertNull(calibrator.getEstimatedBiasFx());
18917 assertNull(calibrator.getEstimatedBiasFy());
18918 assertNull(calibrator.getEstimatedBiasFz());
18919 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
18920 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
18921 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
18922 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
18923 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
18924 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
18925 assertNull(calibrator.getEstimatedMa());
18926 assertNull(calibrator.getEstimatedSx());
18927 assertNull(calibrator.getEstimatedSy());
18928 assertNull(calibrator.getEstimatedSz());
18929 assertNull(calibrator.getEstimatedMxy());
18930 assertNull(calibrator.getEstimatedMxz());
18931 assertNull(calibrator.getEstimatedMyx());
18932 assertNull(calibrator.getEstimatedMyz());
18933 assertNull(calibrator.getEstimatedMzx());
18934 assertNull(calibrator.getEstimatedMzy());
18935 assertNull(calibrator.getEstimatedCovariance());
18936 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
18937 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
18938 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
18939 assertNotNull(calibrator.getGroundTruthGravityNorm());
18940 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
18941 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
18942 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
18943 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
18944 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
18945 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
18946 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
18947
18948
18949 calibrator = null;
18950 try {
18951 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18952 new Matrix(1, 1), ma, this);
18953 fail("IllegalArgumentException expected but not thrown");
18954 } catch (final IllegalArgumentException ignore) {
18955 }
18956 try {
18957 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18958 new Matrix(1, 3), ma, this);
18959 fail("IllegalArgumentException expected but not thrown");
18960 } catch (final IllegalArgumentException ignore) {
18961 }
18962 try {
18963 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18964 ba, new Matrix(1, 3), this);
18965 fail("IllegalArgumentException expected but not thrown");
18966 } catch (final IllegalArgumentException ignore) {
18967 }
18968 try {
18969 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
18970 ba, new Matrix(3, 1), this);
18971 fail("IllegalArgumentException expected but not thrown");
18972 } catch (final IllegalArgumentException ignore) {
18973 }
18974 assertNull(calibrator);
18975 }
18976
18977 @Test
18978 public void testConstructor151() throws WrongSizeException {
18979 final Collection<StandardDeviationBodyKinematics> measurements =
18980 Collections.emptyList();
18981
18982 final Matrix ba = generateBa();
18983 final double[] bias = ba.getBuffer();
18984 final double biasX = ba.getElementAtIndex(0);
18985 final double biasY = ba.getElementAtIndex(1);
18986 final double biasZ = ba.getElementAtIndex(2);
18987
18988 final Matrix ma = generateMaCommonAxis();
18989 final double sx = ma.getElementAt(0, 0);
18990 final double sy = ma.getElementAt(1, 1);
18991 final double sz = ma.getElementAt(2, 2);
18992 final double mxy = ma.getElementAt(0, 1);
18993 final double mxz = ma.getElementAt(0, 2);
18994 final double myx = ma.getElementAt(1, 0);
18995 final double myz = ma.getElementAt(1, 2);
18996 final double mzx = ma.getElementAt(2, 0);
18997 final double mzy = ma.getElementAt(2, 1);
18998
18999 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19000 final double latitude = Math.toRadians(
19001 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19002 final double longitude = Math.toRadians(
19003 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19004 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19005 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19006 final NEDVelocity nedVelocity = new NEDVelocity();
19007 final ECEFPosition ecefPosition = new ECEFPosition();
19008 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19009 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19010 ecefPosition, ecefVelocity);
19011
19012 KnownPositionAccelerometerCalibrator calibrator =
19013 new KnownPositionAccelerometerCalibrator(ecefPosition,
19014 measurements, ba, ma);
19015
19016
19017 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19018 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19019 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19020 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19021 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19022 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19023 final Acceleration bx2 = new Acceleration(0.0,
19024 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19025 calibrator.getInitialBiasXAsAcceleration(bx2);
19026 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19027 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19028 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19029 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19030 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19031 final Acceleration by2 = new Acceleration(0.0,
19032 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19033 calibrator.getInitialBiasYAsAcceleration(by2);
19034 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19035 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19036 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19037 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19038 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19039 final Acceleration bz2 = new Acceleration(0.0,
19040 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19041 calibrator.getInitialBiasZAsAcceleration(bz2);
19042 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19043 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19044 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19045 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19046 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19047 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19048 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19049 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19050 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19051 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19052 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19053 final double[] bias1 = calibrator.getInitialBias();
19054 assertArrayEquals(bias1, bias, 0.0);
19055 final double[] bias2 = new double[3];
19056 calibrator.getInitialBias(bias2);
19057 assertArrayEquals(bias1, bias2, 0.0);
19058 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19059 assertEquals(b1, ba);
19060 final Matrix b2 = new Matrix(3, 1);
19061 calibrator.getInitialBiasAsMatrix(b2);
19062 assertEquals(b1, b2);
19063 final Matrix ma1 = new Matrix(3, 3);
19064 ma1.setSubmatrix(0, 0,
19065 2, 2,
19066 new double[]{sx, myx, mzx,
19067 mxy, sy, mzy,
19068 mxz, myz, sz});
19069 assertEquals(calibrator.getInitialMa(), ma1);
19070 final Matrix ma2 = new Matrix(3, 3);
19071 calibrator.getInitialMa(ma2);
19072 assertEquals(ma1, ma2);
19073 assertSame(calibrator.getEcefPosition(), ecefPosition);
19074 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19075 final NEDPosition nedPosition1 = new NEDPosition();
19076 assertTrue(calibrator.getNedPosition(nedPosition1));
19077 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19078 assertSame(calibrator.getMeasurements(), measurements);
19079 assertFalse(calibrator.isCommonAxisUsed());
19080 assertNull(calibrator.getListener());
19081 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19082 assertFalse(calibrator.isReady());
19083 assertFalse(calibrator.isRunning());
19084 assertNull(calibrator.getEstimatedBiases());
19085 assertFalse(calibrator.getEstimatedBiases(null));
19086 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19087 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19088 assertNull(calibrator.getEstimatedBiasFx());
19089 assertNull(calibrator.getEstimatedBiasFy());
19090 assertNull(calibrator.getEstimatedBiasFz());
19091 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19092 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19093 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19094 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19095 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19096 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19097 assertNull(calibrator.getEstimatedMa());
19098 assertNull(calibrator.getEstimatedSx());
19099 assertNull(calibrator.getEstimatedSy());
19100 assertNull(calibrator.getEstimatedSz());
19101 assertNull(calibrator.getEstimatedMxy());
19102 assertNull(calibrator.getEstimatedMxz());
19103 assertNull(calibrator.getEstimatedMyx());
19104 assertNull(calibrator.getEstimatedMyz());
19105 assertNull(calibrator.getEstimatedMzx());
19106 assertNull(calibrator.getEstimatedMzy());
19107 assertNull(calibrator.getEstimatedCovariance());
19108 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19109 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19110 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19111 assertNotNull(calibrator.getGroundTruthGravityNorm());
19112 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19113 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19114 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19115 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19116 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19117 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19118 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19119
19120
19121 calibrator = null;
19122 try {
19123 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19124 measurements, new Matrix(1, 1), ma);
19125 fail("IllegalArgumentException expected but not thrown");
19126 } catch (final IllegalArgumentException ignore) {
19127 }
19128 try {
19129 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19130 measurements, new Matrix(1, 3), ma);
19131 fail("IllegalArgumentException expected but not thrown");
19132 } catch (final IllegalArgumentException ignore) {
19133 }
19134 try {
19135 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19136 measurements, ba, new Matrix(1, 3));
19137 fail("IllegalArgumentException expected but not thrown");
19138 } catch (final IllegalArgumentException ignore) {
19139 }
19140 try {
19141 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19142 measurements, ba, new Matrix(3, 1));
19143 fail("IllegalArgumentException expected but not thrown");
19144 } catch (final IllegalArgumentException ignore) {
19145 }
19146 assertNull(calibrator);
19147 }
19148
19149 @Test
19150 public void testConstructor152() throws WrongSizeException {
19151 final Collection<StandardDeviationBodyKinematics> measurements =
19152 Collections.emptyList();
19153
19154 final Matrix ba = generateBa();
19155 final double[] bias = ba.getBuffer();
19156 final double biasX = ba.getElementAtIndex(0);
19157 final double biasY = ba.getElementAtIndex(1);
19158 final double biasZ = ba.getElementAtIndex(2);
19159
19160 final Matrix ma = generateMaCommonAxis();
19161 final double sx = ma.getElementAt(0, 0);
19162 final double sy = ma.getElementAt(1, 1);
19163 final double sz = ma.getElementAt(2, 2);
19164 final double mxy = ma.getElementAt(0, 1);
19165 final double mxz = ma.getElementAt(0, 2);
19166 final double myx = ma.getElementAt(1, 0);
19167 final double myz = ma.getElementAt(1, 2);
19168 final double mzx = ma.getElementAt(2, 0);
19169 final double mzy = ma.getElementAt(2, 1);
19170
19171 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19172 final double latitude = Math.toRadians(
19173 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19174 final double longitude = Math.toRadians(
19175 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19176 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19177 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19178 final NEDVelocity nedVelocity = new NEDVelocity();
19179 final ECEFPosition ecefPosition = new ECEFPosition();
19180 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19181 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19182 ecefPosition, ecefVelocity);
19183
19184 KnownPositionAccelerometerCalibrator calibrator =
19185 new KnownPositionAccelerometerCalibrator(ecefPosition,
19186 measurements, ba, ma, this);
19187
19188
19189 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19190 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19191 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19192 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19193 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19194 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19195 final Acceleration bx2 = new Acceleration(0.0,
19196 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19197 calibrator.getInitialBiasXAsAcceleration(bx2);
19198 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19199 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19200 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19201 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19202 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19203 final Acceleration by2 = new Acceleration(0.0,
19204 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19205 calibrator.getInitialBiasYAsAcceleration(by2);
19206 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19207 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19208 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19209 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19210 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19211 final Acceleration bz2 = new Acceleration(0.0,
19212 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19213 calibrator.getInitialBiasZAsAcceleration(bz2);
19214 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19215 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19216 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19217 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19218 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19219 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19220 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19221 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19222 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19223 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19224 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19225 final double[] bias1 = calibrator.getInitialBias();
19226 assertArrayEquals(bias1, bias, 0.0);
19227 final double[] bias2 = new double[3];
19228 calibrator.getInitialBias(bias2);
19229 assertArrayEquals(bias1, bias2, 0.0);
19230 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19231 assertEquals(b1, ba);
19232 final Matrix b2 = new Matrix(3, 1);
19233 calibrator.getInitialBiasAsMatrix(b2);
19234 assertEquals(b1, b2);
19235 final Matrix ma1 = new Matrix(3, 3);
19236 ma1.setSubmatrix(0, 0,
19237 2, 2,
19238 new double[]{sx, myx, mzx,
19239 mxy, sy, mzy,
19240 mxz, myz, sz});
19241 assertEquals(calibrator.getInitialMa(), ma1);
19242 final Matrix ma2 = new Matrix(3, 3);
19243 calibrator.getInitialMa(ma2);
19244 assertEquals(ma1, ma2);
19245 assertSame(calibrator.getEcefPosition(), ecefPosition);
19246 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19247 final NEDPosition nedPosition1 = new NEDPosition();
19248 assertTrue(calibrator.getNedPosition(nedPosition1));
19249 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19250 assertSame(calibrator.getMeasurements(), measurements);
19251 assertFalse(calibrator.isCommonAxisUsed());
19252 assertSame(calibrator.getListener(), this);
19253 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
19254 assertFalse(calibrator.isReady());
19255 assertFalse(calibrator.isRunning());
19256 assertNull(calibrator.getEstimatedBiases());
19257 assertFalse(calibrator.getEstimatedBiases(null));
19258 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19259 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19260 assertNull(calibrator.getEstimatedBiasFx());
19261 assertNull(calibrator.getEstimatedBiasFy());
19262 assertNull(calibrator.getEstimatedBiasFz());
19263 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19264 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19265 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19266 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19267 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19268 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19269 assertNull(calibrator.getEstimatedMa());
19270 assertNull(calibrator.getEstimatedSx());
19271 assertNull(calibrator.getEstimatedSy());
19272 assertNull(calibrator.getEstimatedSz());
19273 assertNull(calibrator.getEstimatedMxy());
19274 assertNull(calibrator.getEstimatedMxz());
19275 assertNull(calibrator.getEstimatedMyx());
19276 assertNull(calibrator.getEstimatedMyz());
19277 assertNull(calibrator.getEstimatedMzx());
19278 assertNull(calibrator.getEstimatedMzy());
19279 assertNull(calibrator.getEstimatedCovariance());
19280 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19281 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19282 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19283 assertNotNull(calibrator.getGroundTruthGravityNorm());
19284 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19285 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19286 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19287 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19288 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19289 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19290 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19291
19292
19293 calibrator = null;
19294 try {
19295 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19296 measurements, new Matrix(1, 1), ma, this);
19297 fail("IllegalArgumentException expected but not thrown");
19298 } catch (final IllegalArgumentException ignore) {
19299 }
19300 try {
19301 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19302 measurements, new Matrix(1, 3), ma, this);
19303 fail("IllegalArgumentException expected but not thrown");
19304 } catch (final IllegalArgumentException ignore) {
19305 }
19306 try {
19307 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19308 measurements, ba, new Matrix(1, 3), this);
19309 fail("IllegalArgumentException expected but not thrown");
19310 } catch (final IllegalArgumentException ignore) {
19311 }
19312 try {
19313 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19314 measurements, ba, new Matrix(3, 1), this);
19315 fail("IllegalArgumentException expected but not thrown");
19316 } catch (final IllegalArgumentException ignore) {
19317 }
19318 assertNull(calibrator);
19319 }
19320
19321 @Test
19322 public void testConstructor153() throws WrongSizeException {
19323 final Matrix ba = generateBa();
19324 final double[] bias = ba.getBuffer();
19325 final double biasX = ba.getElementAtIndex(0);
19326 final double biasY = ba.getElementAtIndex(1);
19327 final double biasZ = ba.getElementAtIndex(2);
19328
19329 final Matrix ma = generateMaCommonAxis();
19330 final double sx = ma.getElementAt(0, 0);
19331 final double sy = ma.getElementAt(1, 1);
19332 final double sz = ma.getElementAt(2, 2);
19333 final double mxy = ma.getElementAt(0, 1);
19334 final double mxz = ma.getElementAt(0, 2);
19335 final double myx = ma.getElementAt(1, 0);
19336 final double myz = ma.getElementAt(1, 2);
19337 final double mzx = ma.getElementAt(2, 0);
19338 final double mzy = ma.getElementAt(2, 1);
19339
19340 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19341 final double latitude = Math.toRadians(
19342 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19343 final double longitude = Math.toRadians(
19344 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19345 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19346 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19347 final NEDVelocity nedVelocity = new NEDVelocity();
19348 final ECEFPosition ecefPosition = new ECEFPosition();
19349 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19350 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19351 ecefPosition, ecefVelocity);
19352
19353 KnownPositionAccelerometerCalibrator calibrator =
19354 new KnownPositionAccelerometerCalibrator(ecefPosition,
19355 true, ba, ma);
19356
19357
19358 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19359 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19360 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19361 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19362 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19363 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19364 final Acceleration bx2 = new Acceleration(0.0,
19365 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19366 calibrator.getInitialBiasXAsAcceleration(bx2);
19367 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19368 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19369 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19370 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19371 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19372 final Acceleration by2 = new Acceleration(0.0,
19373 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19374 calibrator.getInitialBiasYAsAcceleration(by2);
19375 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19376 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19377 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19378 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19379 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19380 final Acceleration bz2 = new Acceleration(0.0,
19381 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19382 calibrator.getInitialBiasZAsAcceleration(bz2);
19383 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19384 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19385 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19386 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19387 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19388 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19389 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19390 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19391 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19392 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19393 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19394 final double[] bias1 = calibrator.getInitialBias();
19395 assertArrayEquals(bias1, bias, 0.0);
19396 final double[] bias2 = new double[3];
19397 calibrator.getInitialBias(bias2);
19398 assertArrayEquals(bias1, bias2, 0.0);
19399 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19400 assertEquals(b1, ba);
19401 final Matrix b2 = new Matrix(3, 1);
19402 calibrator.getInitialBiasAsMatrix(b2);
19403 assertEquals(b1, b2);
19404 final Matrix ma1 = new Matrix(3, 3);
19405 ma1.setSubmatrix(0, 0,
19406 2, 2,
19407 new double[]{sx, myx, mzx,
19408 mxy, sy, mzy,
19409 mxz, myz, sz});
19410 assertEquals(calibrator.getInitialMa(), ma1);
19411 final Matrix ma2 = new Matrix(3, 3);
19412 calibrator.getInitialMa(ma2);
19413 assertEquals(ma1, ma2);
19414 assertSame(calibrator.getEcefPosition(), ecefPosition);
19415 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19416 final NEDPosition nedPosition1 = new NEDPosition();
19417 assertTrue(calibrator.getNedPosition(nedPosition1));
19418 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19419 assertNull(calibrator.getMeasurements());
19420 assertTrue(calibrator.isCommonAxisUsed());
19421 assertNull(calibrator.getListener());
19422 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19423 assertFalse(calibrator.isReady());
19424 assertFalse(calibrator.isRunning());
19425 assertNull(calibrator.getEstimatedBiases());
19426 assertFalse(calibrator.getEstimatedBiases(null));
19427 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19428 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19429 assertNull(calibrator.getEstimatedBiasFx());
19430 assertNull(calibrator.getEstimatedBiasFy());
19431 assertNull(calibrator.getEstimatedBiasFz());
19432 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19433 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19434 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19435 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19436 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19437 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19438 assertNull(calibrator.getEstimatedMa());
19439 assertNull(calibrator.getEstimatedSx());
19440 assertNull(calibrator.getEstimatedSy());
19441 assertNull(calibrator.getEstimatedSz());
19442 assertNull(calibrator.getEstimatedMxy());
19443 assertNull(calibrator.getEstimatedMxz());
19444 assertNull(calibrator.getEstimatedMyx());
19445 assertNull(calibrator.getEstimatedMyz());
19446 assertNull(calibrator.getEstimatedMzx());
19447 assertNull(calibrator.getEstimatedMzy());
19448 assertNull(calibrator.getEstimatedCovariance());
19449 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19450 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19451 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19452 assertNotNull(calibrator.getGroundTruthGravityNorm());
19453 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19454 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19455 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19456 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19457 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19458 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19459 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19460
19461
19462 calibrator = null;
19463 try {
19464 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19465 true, new Matrix(1, 1), ma);
19466 fail("IllegalArgumentException expected but not thrown");
19467 } catch (final IllegalArgumentException ignore) {
19468 }
19469 try {
19470 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19471 true, new Matrix(1, 3), ma);
19472 fail("IllegalArgumentException expected but not thrown");
19473 } catch (final IllegalArgumentException ignore) {
19474 }
19475 try {
19476 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19477 true, ba, new Matrix(1, 3));
19478 fail("IllegalArgumentException expected but not thrown");
19479 } catch (final IllegalArgumentException ignore) {
19480 }
19481 try {
19482 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19483 true, ba, new Matrix(3, 1));
19484 fail("IllegalArgumentException expected but not thrown");
19485 } catch (final IllegalArgumentException ignore) {
19486 }
19487 assertNull(calibrator);
19488 }
19489
19490 @Test
19491 public void testConstructor154() throws WrongSizeException {
19492 final Matrix ba = generateBa();
19493 final double[] bias = ba.getBuffer();
19494 final double biasX = ba.getElementAtIndex(0);
19495 final double biasY = ba.getElementAtIndex(1);
19496 final double biasZ = ba.getElementAtIndex(2);
19497
19498 final Matrix ma = generateMaCommonAxis();
19499 final double sx = ma.getElementAt(0, 0);
19500 final double sy = ma.getElementAt(1, 1);
19501 final double sz = ma.getElementAt(2, 2);
19502 final double mxy = ma.getElementAt(0, 1);
19503 final double mxz = ma.getElementAt(0, 2);
19504 final double myx = ma.getElementAt(1, 0);
19505 final double myz = ma.getElementAt(1, 2);
19506 final double mzx = ma.getElementAt(2, 0);
19507 final double mzy = ma.getElementAt(2, 1);
19508
19509 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19510 final double latitude = Math.toRadians(
19511 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19512 final double longitude = Math.toRadians(
19513 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19514 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19515 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19516 final NEDVelocity nedVelocity = new NEDVelocity();
19517 final ECEFPosition ecefPosition = new ECEFPosition();
19518 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19519 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19520 ecefPosition, ecefVelocity);
19521
19522 KnownPositionAccelerometerCalibrator calibrator =
19523 new KnownPositionAccelerometerCalibrator(ecefPosition,
19524 true, ba, ma, this);
19525
19526
19527 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19528 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19529 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19530 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19531 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19532 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19533 final Acceleration bx2 = new Acceleration(0.0,
19534 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19535 calibrator.getInitialBiasXAsAcceleration(bx2);
19536 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19537 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19538 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19539 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19540 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19541 final Acceleration by2 = new Acceleration(0.0,
19542 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19543 calibrator.getInitialBiasYAsAcceleration(by2);
19544 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19545 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19546 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19547 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19548 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19549 final Acceleration bz2 = new Acceleration(0.0,
19550 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19551 calibrator.getInitialBiasZAsAcceleration(bz2);
19552 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19553 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19554 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19555 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19556 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19557 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19558 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19559 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19560 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19561 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19562 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19563 final double[] bias1 = calibrator.getInitialBias();
19564 assertArrayEquals(bias1, bias, 0.0);
19565 final double[] bias2 = new double[3];
19566 calibrator.getInitialBias(bias2);
19567 assertArrayEquals(bias1, bias2, 0.0);
19568 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19569 assertEquals(b1, ba);
19570 final Matrix b2 = new Matrix(3, 1);
19571 calibrator.getInitialBiasAsMatrix(b2);
19572 assertEquals(b1, b2);
19573 final Matrix ma1 = new Matrix(3, 3);
19574 ma1.setSubmatrix(0, 0,
19575 2, 2,
19576 new double[]{sx, myx, mzx,
19577 mxy, sy, mzy,
19578 mxz, myz, sz});
19579 assertEquals(calibrator.getInitialMa(), ma1);
19580 final Matrix ma2 = new Matrix(3, 3);
19581 calibrator.getInitialMa(ma2);
19582 assertEquals(ma1, ma2);
19583 assertSame(calibrator.getEcefPosition(), ecefPosition);
19584 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19585 final NEDPosition nedPosition1 = new NEDPosition();
19586 assertTrue(calibrator.getNedPosition(nedPosition1));
19587 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19588 assertNull(calibrator.getMeasurements());
19589 assertTrue(calibrator.isCommonAxisUsed());
19590 assertSame(calibrator.getListener(), this);
19591 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19592 assertFalse(calibrator.isReady());
19593 assertFalse(calibrator.isRunning());
19594 assertNull(calibrator.getEstimatedBiases());
19595 assertFalse(calibrator.getEstimatedBiases(null));
19596 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19597 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19598 assertNull(calibrator.getEstimatedBiasFx());
19599 assertNull(calibrator.getEstimatedBiasFy());
19600 assertNull(calibrator.getEstimatedBiasFz());
19601 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19602 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19603 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19604 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19605 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19606 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19607 assertNull(calibrator.getEstimatedMa());
19608 assertNull(calibrator.getEstimatedSx());
19609 assertNull(calibrator.getEstimatedSy());
19610 assertNull(calibrator.getEstimatedSz());
19611 assertNull(calibrator.getEstimatedMxy());
19612 assertNull(calibrator.getEstimatedMxz());
19613 assertNull(calibrator.getEstimatedMyx());
19614 assertNull(calibrator.getEstimatedMyz());
19615 assertNull(calibrator.getEstimatedMzx());
19616 assertNull(calibrator.getEstimatedMzy());
19617 assertNull(calibrator.getEstimatedCovariance());
19618 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19619 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19620 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19621 assertNotNull(calibrator.getGroundTruthGravityNorm());
19622 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19623 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19624 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19625 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19626 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19627 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19628 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19629
19630
19631 calibrator = null;
19632 try {
19633 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19634 true, new Matrix(1, 1), ma, this);
19635 fail("IllegalArgumentException expected but not thrown");
19636 } catch (final IllegalArgumentException ignore) {
19637 }
19638 try {
19639 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19640 true, new Matrix(1, 3), ma, this);
19641 fail("IllegalArgumentException expected but not thrown");
19642 } catch (final IllegalArgumentException ignore) {
19643 }
19644 try {
19645 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19646 true, ba, new Matrix(1, 3), this);
19647 fail("IllegalArgumentException expected but not thrown");
19648 } catch (final IllegalArgumentException ignore) {
19649 }
19650 try {
19651 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19652 true, ba, new Matrix(3, 1), this);
19653 fail("IllegalArgumentException expected but not thrown");
19654 } catch (final IllegalArgumentException ignore) {
19655 }
19656 assertNull(calibrator);
19657 }
19658
19659 @Test
19660 public void testConstructor155() throws WrongSizeException {
19661 final Collection<StandardDeviationBodyKinematics> measurements =
19662 Collections.emptyList();
19663
19664 final Matrix ba = generateBa();
19665 final double[] bias = ba.getBuffer();
19666 final double biasX = ba.getElementAtIndex(0);
19667 final double biasY = ba.getElementAtIndex(1);
19668 final double biasZ = ba.getElementAtIndex(2);
19669
19670 final Matrix ma = generateMaCommonAxis();
19671 final double sx = ma.getElementAt(0, 0);
19672 final double sy = ma.getElementAt(1, 1);
19673 final double sz = ma.getElementAt(2, 2);
19674 final double mxy = ma.getElementAt(0, 1);
19675 final double mxz = ma.getElementAt(0, 2);
19676 final double myx = ma.getElementAt(1, 0);
19677 final double myz = ma.getElementAt(1, 2);
19678 final double mzx = ma.getElementAt(2, 0);
19679 final double mzy = ma.getElementAt(2, 1);
19680
19681 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19682 final double latitude = Math.toRadians(
19683 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19684 final double longitude = Math.toRadians(
19685 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19686 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19687 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19688 final NEDVelocity nedVelocity = new NEDVelocity();
19689 final ECEFPosition ecefPosition = new ECEFPosition();
19690 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19691 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19692 ecefPosition, ecefVelocity);
19693
19694 KnownPositionAccelerometerCalibrator calibrator =
19695 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19696 true, ba, ma);
19697
19698
19699 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19700 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19701 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19702 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19703 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19704 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19705 final Acceleration bx2 = new Acceleration(0.0,
19706 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19707 calibrator.getInitialBiasXAsAcceleration(bx2);
19708 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19709 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19710 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19711 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19712 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19713 final Acceleration by2 = new Acceleration(0.0,
19714 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19715 calibrator.getInitialBiasYAsAcceleration(by2);
19716 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19717 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19718 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19719 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19720 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19721 final Acceleration bz2 = new Acceleration(0.0,
19722 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19723 calibrator.getInitialBiasZAsAcceleration(bz2);
19724 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19725 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19726 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19727 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19728 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19729 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19730 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19731 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19732 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19733 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19734 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19735 final double[] bias1 = calibrator.getInitialBias();
19736 assertArrayEquals(bias1, bias, 0.0);
19737 final double[] bias2 = new double[3];
19738 calibrator.getInitialBias(bias2);
19739 assertArrayEquals(bias1, bias2, 0.0);
19740 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19741 assertEquals(b1, ba);
19742 final Matrix b2 = new Matrix(3, 1);
19743 calibrator.getInitialBiasAsMatrix(b2);
19744 assertEquals(b1, b2);
19745 final Matrix ma1 = new Matrix(3, 3);
19746 ma1.setSubmatrix(0, 0,
19747 2, 2,
19748 new double[]{sx, myx, mzx,
19749 mxy, sy, mzy,
19750 mxz, myz, sz});
19751 assertEquals(calibrator.getInitialMa(), ma1);
19752 final Matrix ma2 = new Matrix(3, 3);
19753 calibrator.getInitialMa(ma2);
19754 assertEquals(ma1, ma2);
19755 assertSame(calibrator.getEcefPosition(), ecefPosition);
19756 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19757 final NEDPosition nedPosition1 = new NEDPosition();
19758 assertTrue(calibrator.getNedPosition(nedPosition1));
19759 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19760 assertSame(calibrator.getMeasurements(), measurements);
19761 assertTrue(calibrator.isCommonAxisUsed());
19762 assertNull(calibrator.getListener());
19763 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19764 assertFalse(calibrator.isReady());
19765 assertFalse(calibrator.isRunning());
19766 assertNull(calibrator.getEstimatedBiases());
19767 assertFalse(calibrator.getEstimatedBiases(null));
19768 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19769 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19770 assertNull(calibrator.getEstimatedBiasFx());
19771 assertNull(calibrator.getEstimatedBiasFy());
19772 assertNull(calibrator.getEstimatedBiasFz());
19773 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19774 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19775 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19776 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19777 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19778 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19779 assertNull(calibrator.getEstimatedMa());
19780 assertNull(calibrator.getEstimatedSx());
19781 assertNull(calibrator.getEstimatedSy());
19782 assertNull(calibrator.getEstimatedSz());
19783 assertNull(calibrator.getEstimatedMxy());
19784 assertNull(calibrator.getEstimatedMxz());
19785 assertNull(calibrator.getEstimatedMyx());
19786 assertNull(calibrator.getEstimatedMyz());
19787 assertNull(calibrator.getEstimatedMzx());
19788 assertNull(calibrator.getEstimatedMzy());
19789 assertNull(calibrator.getEstimatedCovariance());
19790 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19791 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19792 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19793 assertNotNull(calibrator.getGroundTruthGravityNorm());
19794 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19795 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19796 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19797 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19798 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19799 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19800 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19801
19802
19803 calibrator = null;
19804 try {
19805 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19806 measurements, true,
19807 new Matrix(1, 1), ma);
19808 fail("IllegalArgumentException expected but not thrown");
19809 } catch (final IllegalArgumentException ignore) {
19810 }
19811 try {
19812 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19813 measurements, true,
19814 new Matrix(1, 3), ma);
19815 fail("IllegalArgumentException expected but not thrown");
19816 } catch (final IllegalArgumentException ignore) {
19817 }
19818 try {
19819 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19820 measurements, true,
19821 ba, new Matrix(1, 3));
19822 fail("IllegalArgumentException expected but not thrown");
19823 } catch (final IllegalArgumentException ignore) {
19824 }
19825 try {
19826 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19827 measurements, true,
19828 ba, new Matrix(3, 1));
19829 fail("IllegalArgumentException expected but not thrown");
19830 } catch (final IllegalArgumentException ignore) {
19831 }
19832 assertNull(calibrator);
19833 }
19834
19835 @Test
19836 public void testConstructor156() throws WrongSizeException {
19837 final Collection<StandardDeviationBodyKinematics> measurements =
19838 Collections.emptyList();
19839
19840 final Matrix ba = generateBa();
19841 final double[] bias = ba.getBuffer();
19842 final double biasX = ba.getElementAtIndex(0);
19843 final double biasY = ba.getElementAtIndex(1);
19844 final double biasZ = ba.getElementAtIndex(2);
19845
19846 final Matrix ma = generateMaCommonAxis();
19847 final double sx = ma.getElementAt(0, 0);
19848 final double sy = ma.getElementAt(1, 1);
19849 final double sz = ma.getElementAt(2, 2);
19850 final double mxy = ma.getElementAt(0, 1);
19851 final double mxz = ma.getElementAt(0, 2);
19852 final double myx = ma.getElementAt(1, 0);
19853 final double myz = ma.getElementAt(1, 2);
19854 final double mzx = ma.getElementAt(2, 0);
19855 final double mzy = ma.getElementAt(2, 1);
19856
19857 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
19858 final double latitude = Math.toRadians(
19859 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
19860 final double longitude = Math.toRadians(
19861 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
19862 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
19863 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
19864 final NEDVelocity nedVelocity = new NEDVelocity();
19865 final ECEFPosition ecefPosition = new ECEFPosition();
19866 final ECEFVelocity ecefVelocity = new ECEFVelocity();
19867 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
19868 ecefPosition, ecefVelocity);
19869
19870 KnownPositionAccelerometerCalibrator calibrator =
19871 new KnownPositionAccelerometerCalibrator(ecefPosition, measurements,
19872 true, ba, ma, this);
19873
19874
19875 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
19876 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
19877 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
19878 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
19879 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
19880 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19881 final Acceleration bx2 = new Acceleration(0.0,
19882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19883 calibrator.getInitialBiasXAsAcceleration(bx2);
19884 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
19885 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19886 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
19887 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
19888 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19889 final Acceleration by2 = new Acceleration(0.0,
19890 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19891 calibrator.getInitialBiasYAsAcceleration(by2);
19892 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
19893 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19894 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
19895 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
19896 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19897 final Acceleration bz2 = new Acceleration(0.0,
19898 AccelerationUnit.FEET_PER_SQUARED_SECOND);
19899 calibrator.getInitialBiasZAsAcceleration(bz2);
19900 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
19901 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
19902 assertEquals(calibrator.getInitialSx(), sx, 0.0);
19903 assertEquals(calibrator.getInitialSy(), sy, 0.0);
19904 assertEquals(calibrator.getInitialSz(), sz, 0.0);
19905 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
19906 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
19907 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
19908 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
19909 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
19910 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
19911 final double[] bias1 = calibrator.getInitialBias();
19912 assertArrayEquals(bias1, bias, 0.0);
19913 final double[] bias2 = new double[3];
19914 calibrator.getInitialBias(bias2);
19915 assertArrayEquals(bias1, bias2, 0.0);
19916 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
19917 assertEquals(b1, ba);
19918 final Matrix b2 = new Matrix(3, 1);
19919 calibrator.getInitialBiasAsMatrix(b2);
19920 assertEquals(b1, b2);
19921 final Matrix ma1 = new Matrix(3, 3);
19922 ma1.setSubmatrix(0, 0,
19923 2, 2,
19924 new double[]{sx, myx, mzx,
19925 mxy, sy, mzy,
19926 mxz, myz, sz});
19927 assertEquals(calibrator.getInitialMa(), ma1);
19928 final Matrix ma2 = new Matrix(3, 3);
19929 calibrator.getInitialMa(ma2);
19930 assertEquals(ma1, ma2);
19931 assertSame(calibrator.getEcefPosition(), ecefPosition);
19932 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
19933 final NEDPosition nedPosition1 = new NEDPosition();
19934 assertTrue(calibrator.getNedPosition(nedPosition1));
19935 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
19936 assertSame(calibrator.getMeasurements(), measurements);
19937 assertTrue(calibrator.isCommonAxisUsed());
19938 assertSame(calibrator.getListener(), this);
19939 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
19940 assertFalse(calibrator.isReady());
19941 assertFalse(calibrator.isRunning());
19942 assertNull(calibrator.getEstimatedBiases());
19943 assertFalse(calibrator.getEstimatedBiases(null));
19944 assertNull(calibrator.getEstimatedBiasesAsMatrix());
19945 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
19946 assertNull(calibrator.getEstimatedBiasFx());
19947 assertNull(calibrator.getEstimatedBiasFy());
19948 assertNull(calibrator.getEstimatedBiasFz());
19949 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
19950 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
19951 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
19952 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
19953 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
19954 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
19955 assertNull(calibrator.getEstimatedMa());
19956 assertNull(calibrator.getEstimatedSx());
19957 assertNull(calibrator.getEstimatedSy());
19958 assertNull(calibrator.getEstimatedSz());
19959 assertNull(calibrator.getEstimatedMxy());
19960 assertNull(calibrator.getEstimatedMxz());
19961 assertNull(calibrator.getEstimatedMyx());
19962 assertNull(calibrator.getEstimatedMyz());
19963 assertNull(calibrator.getEstimatedMzx());
19964 assertNull(calibrator.getEstimatedMzy());
19965 assertNull(calibrator.getEstimatedCovariance());
19966 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
19967 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
19968 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
19969 assertNotNull(calibrator.getGroundTruthGravityNorm());
19970 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
19971 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
19972 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
19973 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
19974 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
19975 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
19976 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
19977
19978
19979 calibrator = null;
19980 try {
19981 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19982 measurements, true,
19983 new Matrix(1, 1), ma, this);
19984 fail("IllegalArgumentException expected but not thrown");
19985 } catch (final IllegalArgumentException ignore) {
19986 }
19987 try {
19988 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19989 measurements, true,
19990 new Matrix(1, 3), ma, this);
19991 fail("IllegalArgumentException expected but not thrown");
19992 } catch (final IllegalArgumentException ignore) {
19993 }
19994 try {
19995 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
19996 measurements, true,
19997 ba, new Matrix(1, 3), this);
19998 fail("IllegalArgumentException expected but not thrown");
19999 } catch (final IllegalArgumentException ignore) {
20000 }
20001 try {
20002 calibrator = new KnownPositionAccelerometerCalibrator(ecefPosition,
20003 measurements, true,
20004 ba, new Matrix(3, 1), this);
20005 fail("IllegalArgumentException expected but not thrown");
20006 } catch (final IllegalArgumentException ignore) {
20007 }
20008 assertNull(calibrator);
20009 }
20010
20011 @Test
20012 public void testConstructor157() throws WrongSizeException {
20013 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20014 final double latitude = Math.toRadians(
20015 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20016 final double longitude = Math.toRadians(
20017 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20018 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20019 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20020 final NEDVelocity nedVelocity = new NEDVelocity();
20021 final ECEFPosition ecefPosition = new ECEFPosition();
20022 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20023 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20024 ecefPosition, ecefVelocity);
20025
20026 final KnownPositionAccelerometerCalibrator calibrator =
20027 new KnownPositionAccelerometerCalibrator(nedPosition);
20028
20029
20030 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20031 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20032 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20033 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20034 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20035 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20036 final Acceleration bx2 = new Acceleration(0.0,
20037 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20038 calibrator.getInitialBiasXAsAcceleration(bx2);
20039 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20040 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20041 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20042 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20043 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20044 final Acceleration by2 = new Acceleration(0.0,
20045 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20046 calibrator.getInitialBiasYAsAcceleration(by2);
20047 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20048 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20049 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20050 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20051 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20052 final Acceleration bz2 = new Acceleration(0.0,
20053 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20054 calibrator.getInitialBiasZAsAcceleration(bz2);
20055 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20056 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20057 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20058 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20059 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20060 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20061 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20062 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20063 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20064 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20065 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20066 final double[] bias1 = calibrator.getInitialBias();
20067 assertArrayEquals(bias1, new double[3], 0.0);
20068 final double[] bias2 = new double[3];
20069 calibrator.getInitialBias(bias2);
20070 assertArrayEquals(bias1, bias2, 0.0);
20071 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20072 assertEquals(b1, new Matrix(3, 1));
20073 final Matrix b2 = new Matrix(3, 1);
20074 calibrator.getInitialBiasAsMatrix(b2);
20075 assertEquals(b1, b2);
20076 final Matrix ma1 = calibrator.getInitialMa();
20077 assertEquals(ma1, new Matrix(3, 3));
20078 final Matrix ma2 = new Matrix(3, 3);
20079 calibrator.getInitialMa(ma2);
20080 assertEquals(ma1, ma2);
20081 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20082 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20083 final NEDPosition nedPosition1 = new NEDPosition();
20084 assertTrue(calibrator.getNedPosition(nedPosition1));
20085 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20086 assertNull(calibrator.getMeasurements());
20087 assertFalse(calibrator.isCommonAxisUsed());
20088 assertNull(calibrator.getListener());
20089 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20090 assertFalse(calibrator.isReady());
20091 assertFalse(calibrator.isRunning());
20092 assertNull(calibrator.getEstimatedBiases());
20093 assertFalse(calibrator.getEstimatedBiases(null));
20094 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20095 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20096 assertNull(calibrator.getEstimatedBiasFx());
20097 assertNull(calibrator.getEstimatedBiasFy());
20098 assertNull(calibrator.getEstimatedBiasFz());
20099 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20100 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20101 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20102 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20103 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20104 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20105 assertNull(calibrator.getEstimatedMa());
20106 assertNull(calibrator.getEstimatedSx());
20107 assertNull(calibrator.getEstimatedSy());
20108 assertNull(calibrator.getEstimatedSz());
20109 assertNull(calibrator.getEstimatedMxy());
20110 assertNull(calibrator.getEstimatedMxz());
20111 assertNull(calibrator.getEstimatedMyx());
20112 assertNull(calibrator.getEstimatedMyz());
20113 assertNull(calibrator.getEstimatedMzx());
20114 assertNull(calibrator.getEstimatedMzy());
20115 assertNull(calibrator.getEstimatedCovariance());
20116 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20117 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20118 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20119 assertNotNull(calibrator.getGroundTruthGravityNorm());
20120 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20121 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20122 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20123 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20124 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20125 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20126 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20127 }
20128
20129 @Test
20130 public void testConstructor158() throws WrongSizeException {
20131 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20132 final double latitude = Math.toRadians(
20133 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20134 final double longitude = Math.toRadians(
20135 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20136 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20137 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20138 final NEDVelocity nedVelocity = new NEDVelocity();
20139 final ECEFPosition ecefPosition = new ECEFPosition();
20140 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20141 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20142 ecefPosition, ecefVelocity);
20143
20144 final KnownPositionAccelerometerCalibrator calibrator =
20145 new KnownPositionAccelerometerCalibrator(nedPosition, this);
20146
20147
20148 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20149 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20150 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20151 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20152 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20153 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20154 final Acceleration bx2 = new Acceleration(0.0,
20155 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20156 calibrator.getInitialBiasXAsAcceleration(bx2);
20157 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20158 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20159 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20160 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20161 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20162 final Acceleration by2 = new Acceleration(0.0,
20163 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20164 calibrator.getInitialBiasYAsAcceleration(by2);
20165 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20166 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20167 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20168 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20169 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20170 final Acceleration bz2 = new Acceleration(0.0,
20171 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20172 calibrator.getInitialBiasZAsAcceleration(bz2);
20173 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20174 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20175 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20176 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20177 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20178 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20179 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20180 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20181 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20182 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20183 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20184 final double[] bias1 = calibrator.getInitialBias();
20185 assertArrayEquals(bias1, new double[3], 0.0);
20186 final double[] bias2 = new double[3];
20187 calibrator.getInitialBias(bias2);
20188 assertArrayEquals(bias1, bias2, 0.0);
20189 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20190 assertEquals(b1, new Matrix(3, 1));
20191 final Matrix b2 = new Matrix(3, 1);
20192 calibrator.getInitialBiasAsMatrix(b2);
20193 assertEquals(b1, b2);
20194 final Matrix ma1 = calibrator.getInitialMa();
20195 assertEquals(ma1, new Matrix(3, 3));
20196 final Matrix ma2 = new Matrix(3, 3);
20197 calibrator.getInitialMa(ma2);
20198 assertEquals(ma1, ma2);
20199 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20200 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20201 final NEDPosition nedPosition1 = new NEDPosition();
20202 assertTrue(calibrator.getNedPosition(nedPosition1));
20203 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20204 assertNull(calibrator.getMeasurements());
20205 assertFalse(calibrator.isCommonAxisUsed());
20206 assertSame(calibrator.getListener(), this);
20207 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20208 assertFalse(calibrator.isReady());
20209 assertFalse(calibrator.isRunning());
20210 assertNull(calibrator.getEstimatedBiases());
20211 assertFalse(calibrator.getEstimatedBiases(null));
20212 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20213 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20214 assertNull(calibrator.getEstimatedBiasFx());
20215 assertNull(calibrator.getEstimatedBiasFy());
20216 assertNull(calibrator.getEstimatedBiasFz());
20217 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20218 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20219 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20220 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20221 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20222 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20223 assertNull(calibrator.getEstimatedMa());
20224 assertNull(calibrator.getEstimatedSx());
20225 assertNull(calibrator.getEstimatedSy());
20226 assertNull(calibrator.getEstimatedSz());
20227 assertNull(calibrator.getEstimatedMxy());
20228 assertNull(calibrator.getEstimatedMxz());
20229 assertNull(calibrator.getEstimatedMyx());
20230 assertNull(calibrator.getEstimatedMyz());
20231 assertNull(calibrator.getEstimatedMzx());
20232 assertNull(calibrator.getEstimatedMzy());
20233 assertNull(calibrator.getEstimatedCovariance());
20234 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20235 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20236 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20237 assertNotNull(calibrator.getGroundTruthGravityNorm());
20238 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20239 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20240 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20241 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20242 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20243 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20244 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20245 }
20246
20247 @Test
20248 public void testConstructor159() throws WrongSizeException {
20249 final Collection<StandardDeviationBodyKinematics> measurements =
20250 Collections.emptyList();
20251
20252 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20253 final double latitude = Math.toRadians(
20254 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20255 final double longitude = Math.toRadians(
20256 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20257 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20258 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20259 final NEDVelocity nedVelocity = new NEDVelocity();
20260 final ECEFPosition ecefPosition = new ECEFPosition();
20261 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20262 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20263 ecefPosition, ecefVelocity);
20264
20265 final KnownPositionAccelerometerCalibrator calibrator =
20266 new KnownPositionAccelerometerCalibrator(nedPosition, measurements);
20267
20268
20269 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20270 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20271 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20272 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20273 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20274 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20275 final Acceleration bx2 = new Acceleration(0.0,
20276 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20277 calibrator.getInitialBiasXAsAcceleration(bx2);
20278 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20279 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20280 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20281 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20282 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20283 final Acceleration by2 = new Acceleration(0.0,
20284 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20285 calibrator.getInitialBiasYAsAcceleration(by2);
20286 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20287 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20288 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20289 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20290 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20291 final Acceleration bz2 = new Acceleration(0.0,
20292 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20293 calibrator.getInitialBiasZAsAcceleration(bz2);
20294 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20295 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20296 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20297 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20298 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20299 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20300 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20301 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20302 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20303 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20304 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20305 final double[] bias1 = calibrator.getInitialBias();
20306 assertArrayEquals(bias1, new double[3], 0.0);
20307 final double[] bias2 = new double[3];
20308 calibrator.getInitialBias(bias2);
20309 assertArrayEquals(bias1, bias2, 0.0);
20310 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20311 assertEquals(b1, new Matrix(3, 1));
20312 final Matrix b2 = new Matrix(3, 1);
20313 calibrator.getInitialBiasAsMatrix(b2);
20314 assertEquals(b1, b2);
20315 final Matrix ma1 = calibrator.getInitialMa();
20316 assertEquals(ma1, new Matrix(3, 3));
20317 final Matrix ma2 = new Matrix(3, 3);
20318 calibrator.getInitialMa(ma2);
20319 assertEquals(ma1, ma2);
20320 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20321 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20322 final NEDPosition nedPosition1 = new NEDPosition();
20323 assertTrue(calibrator.getNedPosition(nedPosition1));
20324 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20325 assertSame(calibrator.getMeasurements(), measurements);
20326 assertFalse(calibrator.isCommonAxisUsed());
20327 assertNull(calibrator.getListener());
20328 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20329 assertFalse(calibrator.isReady());
20330 assertFalse(calibrator.isRunning());
20331 assertNull(calibrator.getEstimatedBiases());
20332 assertFalse(calibrator.getEstimatedBiases(null));
20333 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20334 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20335 assertNull(calibrator.getEstimatedBiasFx());
20336 assertNull(calibrator.getEstimatedBiasFy());
20337 assertNull(calibrator.getEstimatedBiasFz());
20338 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20339 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20340 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20341 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20342 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20343 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20344 assertNull(calibrator.getEstimatedMa());
20345 assertNull(calibrator.getEstimatedSx());
20346 assertNull(calibrator.getEstimatedSy());
20347 assertNull(calibrator.getEstimatedSz());
20348 assertNull(calibrator.getEstimatedMxy());
20349 assertNull(calibrator.getEstimatedMxz());
20350 assertNull(calibrator.getEstimatedMyx());
20351 assertNull(calibrator.getEstimatedMyz());
20352 assertNull(calibrator.getEstimatedMzx());
20353 assertNull(calibrator.getEstimatedMzy());
20354 assertNull(calibrator.getEstimatedCovariance());
20355 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20356 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20357 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20358 assertNotNull(calibrator.getGroundTruthGravityNorm());
20359 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20360 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20361 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20362 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20363 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20364 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20365 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20366 }
20367
20368 @Test
20369 public void testConstructor160() throws WrongSizeException {
20370 final Collection<StandardDeviationBodyKinematics> measurements =
20371 Collections.emptyList();
20372
20373 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20374 final double latitude = Math.toRadians(
20375 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20376 final double longitude = Math.toRadians(
20377 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20378 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20379 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20380 final NEDVelocity nedVelocity = new NEDVelocity();
20381 final ECEFPosition ecefPosition = new ECEFPosition();
20382 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20383 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20384 ecefPosition, ecefVelocity);
20385
20386 final KnownPositionAccelerometerCalibrator calibrator =
20387 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20388 this);
20389
20390
20391 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20392 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20393 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20394 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20395 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20396 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20397 final Acceleration bx2 = new Acceleration(0.0,
20398 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20399 calibrator.getInitialBiasXAsAcceleration(bx2);
20400 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20401 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20402 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20403 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20404 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20405 final Acceleration by2 = new Acceleration(0.0,
20406 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20407 calibrator.getInitialBiasYAsAcceleration(by2);
20408 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20409 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20410 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20411 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20412 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20413 final Acceleration bz2 = new Acceleration(0.0,
20414 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20415 calibrator.getInitialBiasZAsAcceleration(bz2);
20416 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20417 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20418 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20419 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20420 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20421 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20422 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20423 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20424 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20425 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20426 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20427 final double[] bias1 = calibrator.getInitialBias();
20428 assertArrayEquals(bias1, new double[3], 0.0);
20429 final double[] bias2 = new double[3];
20430 calibrator.getInitialBias(bias2);
20431 assertArrayEquals(bias1, bias2, 0.0);
20432 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20433 assertEquals(b1, new Matrix(3, 1));
20434 final Matrix b2 = new Matrix(3, 1);
20435 calibrator.getInitialBiasAsMatrix(b2);
20436 assertEquals(b1, b2);
20437 final Matrix ma1 = calibrator.getInitialMa();
20438 assertEquals(ma1, new Matrix(3, 3));
20439 final Matrix ma2 = new Matrix(3, 3);
20440 calibrator.getInitialMa(ma2);
20441 assertEquals(ma1, ma2);
20442 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20443 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20444 final NEDPosition nedPosition1 = new NEDPosition();
20445 assertTrue(calibrator.getNedPosition(nedPosition1));
20446 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20447 assertSame(calibrator.getMeasurements(), measurements);
20448 assertFalse(calibrator.isCommonAxisUsed());
20449 assertSame(calibrator.getListener(), this);
20450 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
20451 assertFalse(calibrator.isReady());
20452 assertFalse(calibrator.isRunning());
20453 assertNull(calibrator.getEstimatedBiases());
20454 assertFalse(calibrator.getEstimatedBiases(null));
20455 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20456 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20457 assertNull(calibrator.getEstimatedBiasFx());
20458 assertNull(calibrator.getEstimatedBiasFy());
20459 assertNull(calibrator.getEstimatedBiasFz());
20460 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20461 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20462 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20463 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20464 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20465 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20466 assertNull(calibrator.getEstimatedMa());
20467 assertNull(calibrator.getEstimatedSx());
20468 assertNull(calibrator.getEstimatedSy());
20469 assertNull(calibrator.getEstimatedSz());
20470 assertNull(calibrator.getEstimatedMxy());
20471 assertNull(calibrator.getEstimatedMxz());
20472 assertNull(calibrator.getEstimatedMyx());
20473 assertNull(calibrator.getEstimatedMyz());
20474 assertNull(calibrator.getEstimatedMzx());
20475 assertNull(calibrator.getEstimatedMzy());
20476 assertNull(calibrator.getEstimatedCovariance());
20477 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20478 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20479 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20480 assertNotNull(calibrator.getGroundTruthGravityNorm());
20481 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20482 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20483 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20484 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20485 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20486 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20487 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20488 }
20489
20490 @Test
20491 public void testConstructor161() throws WrongSizeException {
20492 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20493 final double latitude = Math.toRadians(
20494 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20495 final double longitude = Math.toRadians(
20496 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20497 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20498 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20499 final NEDVelocity nedVelocity = new NEDVelocity();
20500 final ECEFPosition ecefPosition = new ECEFPosition();
20501 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20502 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20503 ecefPosition, ecefVelocity);
20504
20505 final KnownPositionAccelerometerCalibrator calibrator =
20506 new KnownPositionAccelerometerCalibrator(nedPosition,
20507 true);
20508
20509
20510 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20511 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20512 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20513 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20514 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20515 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20516 final Acceleration bx2 = new Acceleration(0.0,
20517 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20518 calibrator.getInitialBiasXAsAcceleration(bx2);
20519 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20520 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20521 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20522 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20523 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20524 final Acceleration by2 = new Acceleration(0.0,
20525 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20526 calibrator.getInitialBiasYAsAcceleration(by2);
20527 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20528 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20529 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20530 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20531 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20532 final Acceleration bz2 = new Acceleration(0.0,
20533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20534 calibrator.getInitialBiasZAsAcceleration(bz2);
20535 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20536 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20537 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20538 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20539 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20540 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20541 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20542 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20543 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20544 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20545 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20546 final double[] bias1 = calibrator.getInitialBias();
20547 assertArrayEquals(bias1, new double[3], 0.0);
20548 final double[] bias2 = new double[3];
20549 calibrator.getInitialBias(bias2);
20550 assertArrayEquals(bias1, bias2, 0.0);
20551 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20552 assertEquals(b1, new Matrix(3, 1));
20553 final Matrix b2 = new Matrix(3, 1);
20554 calibrator.getInitialBiasAsMatrix(b2);
20555 assertEquals(b1, b2);
20556 final Matrix ma1 = calibrator.getInitialMa();
20557 assertEquals(ma1, new Matrix(3, 3));
20558 final Matrix ma2 = new Matrix(3, 3);
20559 calibrator.getInitialMa(ma2);
20560 assertEquals(ma1, ma2);
20561 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20562 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20563 final NEDPosition nedPosition1 = new NEDPosition();
20564 assertTrue(calibrator.getNedPosition(nedPosition1));
20565 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20566 assertNull(calibrator.getMeasurements());
20567 assertTrue(calibrator.isCommonAxisUsed());
20568 assertNull(calibrator.getListener());
20569 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20570 assertFalse(calibrator.isReady());
20571 assertFalse(calibrator.isRunning());
20572 assertNull(calibrator.getEstimatedBiases());
20573 assertFalse(calibrator.getEstimatedBiases(null));
20574 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20575 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20576 assertNull(calibrator.getEstimatedBiasFx());
20577 assertNull(calibrator.getEstimatedBiasFy());
20578 assertNull(calibrator.getEstimatedBiasFz());
20579 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20580 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20581 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20582 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20583 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20584 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20585 assertNull(calibrator.getEstimatedMa());
20586 assertNull(calibrator.getEstimatedSx());
20587 assertNull(calibrator.getEstimatedSy());
20588 assertNull(calibrator.getEstimatedSz());
20589 assertNull(calibrator.getEstimatedMxy());
20590 assertNull(calibrator.getEstimatedMxz());
20591 assertNull(calibrator.getEstimatedMyx());
20592 assertNull(calibrator.getEstimatedMyz());
20593 assertNull(calibrator.getEstimatedMzx());
20594 assertNull(calibrator.getEstimatedMzy());
20595 assertNull(calibrator.getEstimatedCovariance());
20596 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20597 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20598 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20599 assertNotNull(calibrator.getGroundTruthGravityNorm());
20600 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20601 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20602 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20603 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20604 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20605 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20606 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20607 }
20608
20609 @Test
20610 public void testConstructor162() throws WrongSizeException {
20611 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20612 final double latitude = Math.toRadians(
20613 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20614 final double longitude = Math.toRadians(
20615 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20616 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20617 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20618 final NEDVelocity nedVelocity = new NEDVelocity();
20619 final ECEFPosition ecefPosition = new ECEFPosition();
20620 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20621 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20622 ecefPosition, ecefVelocity);
20623
20624 final KnownPositionAccelerometerCalibrator calibrator =
20625 new KnownPositionAccelerometerCalibrator(nedPosition,
20626 true, this);
20627
20628
20629 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20630 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20631 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20632 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20633 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20634 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20635 final Acceleration bx2 = new Acceleration(0.0,
20636 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20637 calibrator.getInitialBiasXAsAcceleration(bx2);
20638 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20639 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20640 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20641 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20642 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20643 final Acceleration by2 = new Acceleration(0.0,
20644 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20645 calibrator.getInitialBiasYAsAcceleration(by2);
20646 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20647 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20648 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20649 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20650 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20651 final Acceleration bz2 = new Acceleration(0.0,
20652 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20653 calibrator.getInitialBiasZAsAcceleration(bz2);
20654 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20655 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20656 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20657 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20658 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20659 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20660 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20661 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20662 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20663 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20664 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20665 final double[] bias1 = calibrator.getInitialBias();
20666 assertArrayEquals(bias1, new double[3], 0.0);
20667 final double[] bias2 = new double[3];
20668 calibrator.getInitialBias(bias2);
20669 assertArrayEquals(bias1, bias2, 0.0);
20670 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20671 assertEquals(b1, new Matrix(3, 1));
20672 final Matrix b2 = new Matrix(3, 1);
20673 calibrator.getInitialBiasAsMatrix(b2);
20674 assertEquals(b1, b2);
20675 final Matrix ma1 = calibrator.getInitialMa();
20676 assertEquals(ma1, new Matrix(3, 3));
20677 final Matrix ma2 = new Matrix(3, 3);
20678 calibrator.getInitialMa(ma2);
20679 assertEquals(ma1, ma2);
20680 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20681 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20682 final NEDPosition nedPosition1 = new NEDPosition();
20683 assertTrue(calibrator.getNedPosition(nedPosition1));
20684 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20685 assertNull(calibrator.getMeasurements());
20686 assertTrue(calibrator.isCommonAxisUsed());
20687 assertSame(calibrator.getListener(), this);
20688 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20689 assertFalse(calibrator.isReady());
20690 assertFalse(calibrator.isRunning());
20691 assertNull(calibrator.getEstimatedBiases());
20692 assertFalse(calibrator.getEstimatedBiases(null));
20693 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20694 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20695 assertNull(calibrator.getEstimatedBiasFx());
20696 assertNull(calibrator.getEstimatedBiasFy());
20697 assertNull(calibrator.getEstimatedBiasFz());
20698 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20699 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20700 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20701 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20702 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20703 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20704 assertNull(calibrator.getEstimatedMa());
20705 assertNull(calibrator.getEstimatedSx());
20706 assertNull(calibrator.getEstimatedSy());
20707 assertNull(calibrator.getEstimatedSz());
20708 assertNull(calibrator.getEstimatedMxy());
20709 assertNull(calibrator.getEstimatedMxz());
20710 assertNull(calibrator.getEstimatedMyx());
20711 assertNull(calibrator.getEstimatedMyz());
20712 assertNull(calibrator.getEstimatedMzx());
20713 assertNull(calibrator.getEstimatedMzy());
20714 assertNull(calibrator.getEstimatedCovariance());
20715 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20716 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20717 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20718 assertNotNull(calibrator.getGroundTruthGravityNorm());
20719 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20720 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20721 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20722 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20723 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20724 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20725 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20726 }
20727
20728 @Test
20729 public void testConstructor163() throws WrongSizeException {
20730 final Collection<StandardDeviationBodyKinematics> measurements =
20731 Collections.emptyList();
20732 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20733 final double latitude = Math.toRadians(
20734 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20735 final double longitude = Math.toRadians(
20736 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20737 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20738 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20739 final NEDVelocity nedVelocity = new NEDVelocity();
20740 final ECEFPosition ecefPosition = new ECEFPosition();
20741 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20742 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20743 ecefPosition, ecefVelocity);
20744
20745 final KnownPositionAccelerometerCalibrator calibrator =
20746 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20747 true);
20748
20749
20750 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20751 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20752 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20753 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20754 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20755 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20756 final Acceleration bx2 = new Acceleration(0.0,
20757 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20758 calibrator.getInitialBiasXAsAcceleration(bx2);
20759 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20760 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20761 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20762 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20763 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20764 final Acceleration by2 = new Acceleration(0.0,
20765 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20766 calibrator.getInitialBiasYAsAcceleration(by2);
20767 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20768 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20769 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20770 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20771 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20772 final Acceleration bz2 = new Acceleration(0.0,
20773 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20774 calibrator.getInitialBiasZAsAcceleration(bz2);
20775 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20776 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20777 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20778 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20779 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20780 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20781 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20782 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20783 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20784 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20785 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20786 final double[] bias1 = calibrator.getInitialBias();
20787 assertArrayEquals(bias1, new double[3], 0.0);
20788 final double[] bias2 = new double[3];
20789 calibrator.getInitialBias(bias2);
20790 assertArrayEquals(bias1, bias2, 0.0);
20791 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20792 assertEquals(b1, new Matrix(3, 1));
20793 final Matrix b2 = new Matrix(3, 1);
20794 calibrator.getInitialBiasAsMatrix(b2);
20795 assertEquals(b1, b2);
20796 final Matrix ma1 = calibrator.getInitialMa();
20797 assertEquals(ma1, new Matrix(3, 3));
20798 final Matrix ma2 = new Matrix(3, 3);
20799 calibrator.getInitialMa(ma2);
20800 assertEquals(ma1, ma2);
20801 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20802 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20803 final NEDPosition nedPosition1 = new NEDPosition();
20804 assertTrue(calibrator.getNedPosition(nedPosition1));
20805 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20806 assertSame(calibrator.getMeasurements(), measurements);
20807 assertTrue(calibrator.isCommonAxisUsed());
20808 assertNull(calibrator.getListener());
20809 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20810 assertFalse(calibrator.isReady());
20811 assertFalse(calibrator.isRunning());
20812 assertNull(calibrator.getEstimatedBiases());
20813 assertFalse(calibrator.getEstimatedBiases(null));
20814 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20815 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20816 assertNull(calibrator.getEstimatedBiasFx());
20817 assertNull(calibrator.getEstimatedBiasFy());
20818 assertNull(calibrator.getEstimatedBiasFz());
20819 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20820 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20821 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20822 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20823 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20824 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20825 assertNull(calibrator.getEstimatedMa());
20826 assertNull(calibrator.getEstimatedSx());
20827 assertNull(calibrator.getEstimatedSy());
20828 assertNull(calibrator.getEstimatedSz());
20829 assertNull(calibrator.getEstimatedMxy());
20830 assertNull(calibrator.getEstimatedMxz());
20831 assertNull(calibrator.getEstimatedMyx());
20832 assertNull(calibrator.getEstimatedMyz());
20833 assertNull(calibrator.getEstimatedMzx());
20834 assertNull(calibrator.getEstimatedMzy());
20835 assertNull(calibrator.getEstimatedCovariance());
20836 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20837 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20838 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20839 assertNotNull(calibrator.getGroundTruthGravityNorm());
20840 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20841 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20842 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20843 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20844 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20845 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20846 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20847 }
20848
20849 @Test
20850 public void testConstructor164() throws WrongSizeException {
20851 final Collection<StandardDeviationBodyKinematics> measurements =
20852 Collections.emptyList();
20853 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20854 final double latitude = Math.toRadians(
20855 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20856 final double longitude = Math.toRadians(
20857 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20858 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20859 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20860 final NEDVelocity nedVelocity = new NEDVelocity();
20861 final ECEFPosition ecefPosition = new ECEFPosition();
20862 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20863 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20864 ecefPosition, ecefVelocity);
20865
20866 final KnownPositionAccelerometerCalibrator calibrator =
20867 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
20868 true, this);
20869
20870
20871 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
20872 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
20873 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
20874 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20875 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
20876 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20877 final Acceleration bx2 = new Acceleration(0.0,
20878 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20879 calibrator.getInitialBiasXAsAcceleration(bx2);
20880 assertEquals(bx2.getValue().doubleValue(), 0.0, 0.0);
20881 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20882 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
20883 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
20884 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20885 final Acceleration by2 = new Acceleration(0.0,
20886 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20887 calibrator.getInitialBiasYAsAcceleration(by2);
20888 assertEquals(by2.getValue().doubleValue(), 0.0, 0.0);
20889 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20890 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
20891 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
20892 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20893 final Acceleration bz2 = new Acceleration(0.0,
20894 AccelerationUnit.FEET_PER_SQUARED_SECOND);
20895 calibrator.getInitialBiasZAsAcceleration(bz2);
20896 assertEquals(bz2.getValue().doubleValue(), 0.0, 0.0);
20897 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
20898 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
20899 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
20900 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
20901 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
20902 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
20903 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
20904 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
20905 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
20906 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
20907 final double[] bias1 = calibrator.getInitialBias();
20908 assertArrayEquals(bias1, new double[3], 0.0);
20909 final double[] bias2 = new double[3];
20910 calibrator.getInitialBias(bias2);
20911 assertArrayEquals(bias1, bias2, 0.0);
20912 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
20913 assertEquals(b1, new Matrix(3, 1));
20914 final Matrix b2 = new Matrix(3, 1);
20915 calibrator.getInitialBiasAsMatrix(b2);
20916 assertEquals(b1, b2);
20917 final Matrix ma1 = calibrator.getInitialMa();
20918 assertEquals(ma1, new Matrix(3, 3));
20919 final Matrix ma2 = new Matrix(3, 3);
20920 calibrator.getInitialMa(ma2);
20921 assertEquals(ma1, ma2);
20922 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
20923 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
20924 final NEDPosition nedPosition1 = new NEDPosition();
20925 assertTrue(calibrator.getNedPosition(nedPosition1));
20926 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
20927 assertSame(calibrator.getMeasurements(), measurements);
20928 assertTrue(calibrator.isCommonAxisUsed());
20929 assertSame(calibrator.getListener(), this);
20930 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
20931 assertFalse(calibrator.isReady());
20932 assertFalse(calibrator.isRunning());
20933 assertNull(calibrator.getEstimatedBiases());
20934 assertFalse(calibrator.getEstimatedBiases(null));
20935 assertNull(calibrator.getEstimatedBiasesAsMatrix());
20936 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
20937 assertNull(calibrator.getEstimatedBiasFx());
20938 assertNull(calibrator.getEstimatedBiasFy());
20939 assertNull(calibrator.getEstimatedBiasFz());
20940 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
20941 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
20942 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
20943 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
20944 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
20945 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
20946 assertNull(calibrator.getEstimatedMa());
20947 assertNull(calibrator.getEstimatedSx());
20948 assertNull(calibrator.getEstimatedSy());
20949 assertNull(calibrator.getEstimatedSz());
20950 assertNull(calibrator.getEstimatedMxy());
20951 assertNull(calibrator.getEstimatedMxz());
20952 assertNull(calibrator.getEstimatedMyx());
20953 assertNull(calibrator.getEstimatedMyz());
20954 assertNull(calibrator.getEstimatedMzx());
20955 assertNull(calibrator.getEstimatedMzy());
20956 assertNull(calibrator.getEstimatedCovariance());
20957 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
20958 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
20959 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
20960 assertNotNull(calibrator.getGroundTruthGravityNorm());
20961 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
20962 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
20963 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
20964 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
20965 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
20966 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
20967 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
20968 }
20969
20970 @Test
20971 public void testConstructor165() throws WrongSizeException {
20972 final Matrix ba = generateBa();
20973 final double biasX = ba.getElementAtIndex(0);
20974 final double biasY = ba.getElementAtIndex(1);
20975 final double biasZ = ba.getElementAtIndex(2);
20976
20977 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
20978 final double latitude = Math.toRadians(
20979 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
20980 final double longitude = Math.toRadians(
20981 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
20982 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
20983 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
20984 final NEDVelocity nedVelocity = new NEDVelocity();
20985 final ECEFPosition ecefPosition = new ECEFPosition();
20986 final ECEFVelocity ecefVelocity = new ECEFVelocity();
20987 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
20988 ecefPosition, ecefVelocity);
20989
20990 final KnownPositionAccelerometerCalibrator calibrator =
20991 new KnownPositionAccelerometerCalibrator(nedPosition,
20992 biasX, biasY, biasZ);
20993
20994
20995 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
20996 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
20997 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
20998 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
20999 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21000 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21001 final Acceleration bx2 = new Acceleration(0.0,
21002 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21003 calibrator.getInitialBiasXAsAcceleration(bx2);
21004 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21005 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21006 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21007 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21008 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21009 final Acceleration by2 = new Acceleration(0.0,
21010 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21011 calibrator.getInitialBiasYAsAcceleration(by2);
21012 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21013 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21014 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21015 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21016 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21017 final Acceleration bz2 = new Acceleration(0.0,
21018 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21019 calibrator.getInitialBiasZAsAcceleration(bz2);
21020 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21021 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21022 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21023 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21024 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21025 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21026 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21027 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21028 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21029 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21030 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21031 final double[] bias1 = calibrator.getInitialBias();
21032 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21033 final double[] bias2 = new double[3];
21034 calibrator.getInitialBias(bias2);
21035 assertArrayEquals(bias1, bias2, 0.0);
21036 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21037 assertEquals(b1, ba);
21038 final Matrix b2 = new Matrix(3, 1);
21039 calibrator.getInitialBiasAsMatrix(b2);
21040 assertEquals(b1, b2);
21041 final Matrix ma1 = calibrator.getInitialMa();
21042 assertEquals(ma1, new Matrix(3, 3));
21043 final Matrix ma2 = new Matrix(3, 3);
21044 calibrator.getInitialMa(ma2);
21045 assertEquals(ma1, ma2);
21046 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21047 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21048 final NEDPosition nedPosition1 = new NEDPosition();
21049 assertTrue(calibrator.getNedPosition(nedPosition1));
21050 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21051 assertNull(calibrator.getMeasurements());
21052 assertFalse(calibrator.isCommonAxisUsed());
21053 assertNull(calibrator.getListener());
21054 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21055 assertFalse(calibrator.isReady());
21056 assertFalse(calibrator.isRunning());
21057 assertNull(calibrator.getEstimatedBiases());
21058 assertFalse(calibrator.getEstimatedBiases(null));
21059 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21060 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21061 assertNull(calibrator.getEstimatedBiasFx());
21062 assertNull(calibrator.getEstimatedBiasFy());
21063 assertNull(calibrator.getEstimatedBiasFz());
21064 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21065 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21066 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21067 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21068 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21069 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21070 assertNull(calibrator.getEstimatedMa());
21071 assertNull(calibrator.getEstimatedSx());
21072 assertNull(calibrator.getEstimatedSy());
21073 assertNull(calibrator.getEstimatedSz());
21074 assertNull(calibrator.getEstimatedMxy());
21075 assertNull(calibrator.getEstimatedMxz());
21076 assertNull(calibrator.getEstimatedMyx());
21077 assertNull(calibrator.getEstimatedMyz());
21078 assertNull(calibrator.getEstimatedMzx());
21079 assertNull(calibrator.getEstimatedMzy());
21080 assertNull(calibrator.getEstimatedCovariance());
21081 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21082 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21083 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21084 assertNotNull(calibrator.getGroundTruthGravityNorm());
21085 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21086 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21087 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21088 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21089 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21090 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21091 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21092 }
21093
21094 @Test
21095 public void testConstructor166() throws WrongSizeException {
21096 final Matrix ba = generateBa();
21097 final double biasX = ba.getElementAtIndex(0);
21098 final double biasY = ba.getElementAtIndex(1);
21099 final double biasZ = ba.getElementAtIndex(2);
21100
21101 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21102 final double latitude = Math.toRadians(
21103 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21104 final double longitude = Math.toRadians(
21105 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21106 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21107 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21108 final NEDVelocity nedVelocity = new NEDVelocity();
21109 final ECEFPosition ecefPosition = new ECEFPosition();
21110 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21111 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21112 ecefPosition, ecefVelocity);
21113
21114 final KnownPositionAccelerometerCalibrator calibrator =
21115 new KnownPositionAccelerometerCalibrator(nedPosition,
21116 biasX, biasY, biasZ, this);
21117
21118
21119 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21120 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21121 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21122 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21123 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21124 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21125 final Acceleration bx2 = new Acceleration(0.0,
21126 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21127 calibrator.getInitialBiasXAsAcceleration(bx2);
21128 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21129 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21130 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21131 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21132 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21133 final Acceleration by2 = new Acceleration(0.0,
21134 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21135 calibrator.getInitialBiasYAsAcceleration(by2);
21136 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21137 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21138 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21139 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21140 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21141 final Acceleration bz2 = new Acceleration(0.0,
21142 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21143 calibrator.getInitialBiasZAsAcceleration(bz2);
21144 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21145 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21146 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21147 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21148 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21149 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21150 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21151 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21152 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21153 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21154 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21155 final double[] bias1 = calibrator.getInitialBias();
21156 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21157 final double[] bias2 = new double[3];
21158 calibrator.getInitialBias(bias2);
21159 assertArrayEquals(bias1, bias2, 0.0);
21160 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21161 assertEquals(b1, ba);
21162 final Matrix b2 = new Matrix(3, 1);
21163 calibrator.getInitialBiasAsMatrix(b2);
21164 assertEquals(b1, b2);
21165 final Matrix ma1 = calibrator.getInitialMa();
21166 assertEquals(ma1, new Matrix(3, 3));
21167 final Matrix ma2 = new Matrix(3, 3);
21168 calibrator.getInitialMa(ma2);
21169 assertEquals(ma1, ma2);
21170 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21171 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21172 final NEDPosition nedPosition1 = new NEDPosition();
21173 assertTrue(calibrator.getNedPosition(nedPosition1));
21174 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21175 assertNull(calibrator.getMeasurements());
21176 assertFalse(calibrator.isCommonAxisUsed());
21177 assertSame(calibrator.getListener(), this);
21178 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21179 assertFalse(calibrator.isReady());
21180 assertFalse(calibrator.isRunning());
21181 assertNull(calibrator.getEstimatedBiases());
21182 assertFalse(calibrator.getEstimatedBiases(null));
21183 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21184 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21185 assertNull(calibrator.getEstimatedBiasFx());
21186 assertNull(calibrator.getEstimatedBiasFy());
21187 assertNull(calibrator.getEstimatedBiasFz());
21188 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21189 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21190 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21191 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21192 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21193 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21194 assertNull(calibrator.getEstimatedMa());
21195 assertNull(calibrator.getEstimatedSx());
21196 assertNull(calibrator.getEstimatedSy());
21197 assertNull(calibrator.getEstimatedSz());
21198 assertNull(calibrator.getEstimatedMxy());
21199 assertNull(calibrator.getEstimatedMxz());
21200 assertNull(calibrator.getEstimatedMyx());
21201 assertNull(calibrator.getEstimatedMyz());
21202 assertNull(calibrator.getEstimatedMzx());
21203 assertNull(calibrator.getEstimatedMzy());
21204 assertNull(calibrator.getEstimatedCovariance());
21205 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21206 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21207 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21208 assertNotNull(calibrator.getGroundTruthGravityNorm());
21209 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21210 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21211 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21212 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21213 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21214 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21215 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21216 }
21217
21218 @Test
21219 public void testConstructor167() throws WrongSizeException {
21220 final Collection<StandardDeviationBodyKinematics> measurements =
21221 Collections.emptyList();
21222
21223 final Matrix ba = generateBa();
21224 final double biasX = ba.getElementAtIndex(0);
21225 final double biasY = ba.getElementAtIndex(1);
21226 final double biasZ = ba.getElementAtIndex(2);
21227
21228 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21229 final double latitude = Math.toRadians(
21230 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21231 final double longitude = Math.toRadians(
21232 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21233 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21234 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21235 final NEDVelocity nedVelocity = new NEDVelocity();
21236 final ECEFPosition ecefPosition = new ECEFPosition();
21237 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21238 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21239 ecefPosition, ecefVelocity);
21240
21241 final KnownPositionAccelerometerCalibrator calibrator =
21242 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21243 biasX, biasY, biasZ);
21244
21245
21246 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21247 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21248 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21249 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21250 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21251 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21252 final Acceleration bx2 = new Acceleration(0.0,
21253 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21254 calibrator.getInitialBiasXAsAcceleration(bx2);
21255 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21256 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21257 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21258 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21259 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21260 final Acceleration by2 = new Acceleration(0.0,
21261 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21262 calibrator.getInitialBiasYAsAcceleration(by2);
21263 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21264 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21265 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21266 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21267 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21268 final Acceleration bz2 = new Acceleration(0.0,
21269 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21270 calibrator.getInitialBiasZAsAcceleration(bz2);
21271 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21272 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21273 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21274 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21275 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21276 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21277 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21278 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21279 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21280 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21281 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21282 final double[] bias1 = calibrator.getInitialBias();
21283 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21284 final double[] bias2 = new double[3];
21285 calibrator.getInitialBias(bias2);
21286 assertArrayEquals(bias1, bias2, 0.0);
21287 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21288 assertEquals(b1, ba);
21289 final Matrix b2 = new Matrix(3, 1);
21290 calibrator.getInitialBiasAsMatrix(b2);
21291 assertEquals(b1, b2);
21292 final Matrix ma1 = calibrator.getInitialMa();
21293 assertEquals(ma1, new Matrix(3, 3));
21294 final Matrix ma2 = new Matrix(3, 3);
21295 calibrator.getInitialMa(ma2);
21296 assertEquals(ma1, ma2);
21297 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21298 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21299 final NEDPosition nedPosition1 = new NEDPosition();
21300 assertTrue(calibrator.getNedPosition(nedPosition1));
21301 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21302 assertSame(calibrator.getMeasurements(), measurements);
21303 assertFalse(calibrator.isCommonAxisUsed());
21304 assertNull(calibrator.getListener());
21305 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21306 assertFalse(calibrator.isReady());
21307 assertFalse(calibrator.isRunning());
21308 assertNull(calibrator.getEstimatedBiases());
21309 assertFalse(calibrator.getEstimatedBiases(null));
21310 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21311 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21312 assertNull(calibrator.getEstimatedBiasFx());
21313 assertNull(calibrator.getEstimatedBiasFy());
21314 assertNull(calibrator.getEstimatedBiasFz());
21315 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21316 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21317 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21318 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21319 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21320 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21321 assertNull(calibrator.getEstimatedMa());
21322 assertNull(calibrator.getEstimatedSx());
21323 assertNull(calibrator.getEstimatedSy());
21324 assertNull(calibrator.getEstimatedSz());
21325 assertNull(calibrator.getEstimatedMxy());
21326 assertNull(calibrator.getEstimatedMxz());
21327 assertNull(calibrator.getEstimatedMyx());
21328 assertNull(calibrator.getEstimatedMyz());
21329 assertNull(calibrator.getEstimatedMzx());
21330 assertNull(calibrator.getEstimatedMzy());
21331 assertNull(calibrator.getEstimatedCovariance());
21332 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21333 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21334 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21335 assertNotNull(calibrator.getGroundTruthGravityNorm());
21336 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21337 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21338 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21339 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21340 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21341 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21342 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21343 }
21344
21345 @Test
21346 public void testConstructor168() throws WrongSizeException {
21347 final Collection<StandardDeviationBodyKinematics> measurements =
21348 Collections.emptyList();
21349
21350 final Matrix ba = generateBa();
21351 final double biasX = ba.getElementAtIndex(0);
21352 final double biasY = ba.getElementAtIndex(1);
21353 final double biasZ = ba.getElementAtIndex(2);
21354
21355 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21356 final double latitude = Math.toRadians(
21357 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21358 final double longitude = Math.toRadians(
21359 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21360 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21361 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21362 final NEDVelocity nedVelocity = new NEDVelocity();
21363 final ECEFPosition ecefPosition = new ECEFPosition();
21364 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21365 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21366 ecefPosition, ecefVelocity);
21367
21368 final KnownPositionAccelerometerCalibrator calibrator =
21369 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21370 biasX, biasY, biasZ, this);
21371
21372
21373 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21374 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21375 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21376 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21377 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21378 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21379 final Acceleration bx2 = new Acceleration(0.0,
21380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21381 calibrator.getInitialBiasXAsAcceleration(bx2);
21382 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21383 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21384 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21385 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21386 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21387 final Acceleration by2 = new Acceleration(0.0,
21388 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21389 calibrator.getInitialBiasYAsAcceleration(by2);
21390 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21391 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21392 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21393 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21394 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21395 final Acceleration bz2 = new Acceleration(0.0,
21396 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21397 calibrator.getInitialBiasZAsAcceleration(bz2);
21398 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21399 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21400 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21401 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21402 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21403 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21404 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21405 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21406 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21407 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21408 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21409 final double[] bias1 = calibrator.getInitialBias();
21410 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21411 final double[] bias2 = new double[3];
21412 calibrator.getInitialBias(bias2);
21413 assertArrayEquals(bias1, bias2, 0.0);
21414 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21415 assertEquals(b1, ba);
21416 final Matrix b2 = new Matrix(3, 1);
21417 calibrator.getInitialBiasAsMatrix(b2);
21418 assertEquals(b1, b2);
21419 final Matrix ma1 = calibrator.getInitialMa();
21420 assertEquals(ma1, new Matrix(3, 3));
21421 final Matrix ma2 = new Matrix(3, 3);
21422 calibrator.getInitialMa(ma2);
21423 assertEquals(ma1, ma2);
21424 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21425 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21426 final NEDPosition nedPosition1 = new NEDPosition();
21427 assertTrue(calibrator.getNedPosition(nedPosition1));
21428 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21429 assertSame(calibrator.getMeasurements(), measurements);
21430 assertFalse(calibrator.isCommonAxisUsed());
21431 assertSame(calibrator.getListener(), this);
21432 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
21433 assertFalse(calibrator.isReady());
21434 assertFalse(calibrator.isRunning());
21435 assertNull(calibrator.getEstimatedBiases());
21436 assertFalse(calibrator.getEstimatedBiases(null));
21437 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21438 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21439 assertNull(calibrator.getEstimatedBiasFx());
21440 assertNull(calibrator.getEstimatedBiasFy());
21441 assertNull(calibrator.getEstimatedBiasFz());
21442 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21443 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21444 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21445 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21446 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21447 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21448 assertNull(calibrator.getEstimatedMa());
21449 assertNull(calibrator.getEstimatedSx());
21450 assertNull(calibrator.getEstimatedSy());
21451 assertNull(calibrator.getEstimatedSz());
21452 assertNull(calibrator.getEstimatedMxy());
21453 assertNull(calibrator.getEstimatedMxz());
21454 assertNull(calibrator.getEstimatedMyx());
21455 assertNull(calibrator.getEstimatedMyz());
21456 assertNull(calibrator.getEstimatedMzx());
21457 assertNull(calibrator.getEstimatedMzy());
21458 assertNull(calibrator.getEstimatedCovariance());
21459 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21460 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21461 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21462 assertNotNull(calibrator.getGroundTruthGravityNorm());
21463 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21464 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21465 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21466 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21467 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21468 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21469 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21470 }
21471
21472 @Test
21473 public void testConstructor169() throws WrongSizeException {
21474 final Matrix ba = generateBa();
21475 final double biasX = ba.getElementAtIndex(0);
21476 final double biasY = ba.getElementAtIndex(1);
21477 final double biasZ = ba.getElementAtIndex(2);
21478
21479 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21480 final double latitude = Math.toRadians(
21481 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21482 final double longitude = Math.toRadians(
21483 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21484 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21485 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21486 final NEDVelocity nedVelocity = new NEDVelocity();
21487 final ECEFPosition ecefPosition = new ECEFPosition();
21488 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21489 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21490 ecefPosition, ecefVelocity);
21491
21492 final KnownPositionAccelerometerCalibrator calibrator =
21493 new KnownPositionAccelerometerCalibrator(nedPosition,
21494 true, biasX, biasY, biasZ);
21495
21496
21497 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21498 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21499 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21500 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21501 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21502 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21503 final Acceleration bx2 = new Acceleration(0.0,
21504 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21505 calibrator.getInitialBiasXAsAcceleration(bx2);
21506 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21507 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21508 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21509 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21510 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21511 final Acceleration by2 = new Acceleration(0.0,
21512 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21513 calibrator.getInitialBiasYAsAcceleration(by2);
21514 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21515 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21516 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21517 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21518 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21519 final Acceleration bz2 = new Acceleration(0.0,
21520 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21521 calibrator.getInitialBiasZAsAcceleration(bz2);
21522 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21523 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21524 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21525 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21526 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21527 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21528 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21529 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21530 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21531 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21532 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21533 final double[] bias1 = calibrator.getInitialBias();
21534 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21535 final double[] bias2 = new double[3];
21536 calibrator.getInitialBias(bias2);
21537 assertArrayEquals(bias1, bias2, 0.0);
21538 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21539 assertEquals(b1, ba);
21540 final Matrix b2 = new Matrix(3, 1);
21541 calibrator.getInitialBiasAsMatrix(b2);
21542 assertEquals(b1, b2);
21543 final Matrix ma1 = calibrator.getInitialMa();
21544 assertEquals(ma1, new Matrix(3, 3));
21545 final Matrix ma2 = new Matrix(3, 3);
21546 calibrator.getInitialMa(ma2);
21547 assertEquals(ma1, ma2);
21548 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21549 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21550 final NEDPosition nedPosition1 = new NEDPosition();
21551 assertTrue(calibrator.getNedPosition(nedPosition1));
21552 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21553 assertNull(calibrator.getMeasurements());
21554 assertTrue(calibrator.isCommonAxisUsed());
21555 assertNull(calibrator.getListener());
21556 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21557 assertFalse(calibrator.isReady());
21558 assertFalse(calibrator.isRunning());
21559 assertNull(calibrator.getEstimatedBiases());
21560 assertFalse(calibrator.getEstimatedBiases(null));
21561 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21562 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21563 assertNull(calibrator.getEstimatedBiasFx());
21564 assertNull(calibrator.getEstimatedBiasFy());
21565 assertNull(calibrator.getEstimatedBiasFz());
21566 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21567 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21568 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21569 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21570 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21571 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21572 assertNull(calibrator.getEstimatedMa());
21573 assertNull(calibrator.getEstimatedSx());
21574 assertNull(calibrator.getEstimatedSy());
21575 assertNull(calibrator.getEstimatedSz());
21576 assertNull(calibrator.getEstimatedMxy());
21577 assertNull(calibrator.getEstimatedMxz());
21578 assertNull(calibrator.getEstimatedMyx());
21579 assertNull(calibrator.getEstimatedMyz());
21580 assertNull(calibrator.getEstimatedMzx());
21581 assertNull(calibrator.getEstimatedMzy());
21582 assertNull(calibrator.getEstimatedCovariance());
21583 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21584 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21585 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21586 assertNotNull(calibrator.getGroundTruthGravityNorm());
21587 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21588 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21589 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21590 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21591 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21592 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21593 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21594 }
21595
21596 @Test
21597 public void testConstructor170() throws WrongSizeException {
21598 final Matrix ba = generateBa();
21599 final double biasX = ba.getElementAtIndex(0);
21600 final double biasY = ba.getElementAtIndex(1);
21601 final double biasZ = ba.getElementAtIndex(2);
21602
21603 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21604 final double latitude = Math.toRadians(
21605 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21606 final double longitude = Math.toRadians(
21607 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21608 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21609 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21610 final NEDVelocity nedVelocity = new NEDVelocity();
21611 final ECEFPosition ecefPosition = new ECEFPosition();
21612 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21613 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21614 ecefPosition, ecefVelocity);
21615
21616 final KnownPositionAccelerometerCalibrator calibrator =
21617 new KnownPositionAccelerometerCalibrator(nedPosition,
21618 true, biasX, biasY, biasZ, this);
21619
21620
21621 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21622 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21623 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21624 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21625 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21626 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21627 final Acceleration bx2 = new Acceleration(0.0,
21628 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21629 calibrator.getInitialBiasXAsAcceleration(bx2);
21630 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21631 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21632 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21633 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21634 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21635 final Acceleration by2 = new Acceleration(0.0,
21636 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21637 calibrator.getInitialBiasYAsAcceleration(by2);
21638 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21639 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21640 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21641 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21642 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21643 final Acceleration bz2 = new Acceleration(0.0,
21644 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21645 calibrator.getInitialBiasZAsAcceleration(bz2);
21646 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21647 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21648 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21649 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21650 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21651 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21652 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21653 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21654 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21655 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21656 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21657 final double[] bias1 = calibrator.getInitialBias();
21658 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21659 final double[] bias2 = new double[3];
21660 calibrator.getInitialBias(bias2);
21661 assertArrayEquals(bias1, bias2, 0.0);
21662 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21663 assertEquals(b1, ba);
21664 final Matrix b2 = new Matrix(3, 1);
21665 calibrator.getInitialBiasAsMatrix(b2);
21666 assertEquals(b1, b2);
21667 final Matrix ma1 = calibrator.getInitialMa();
21668 assertEquals(ma1, new Matrix(3, 3));
21669 final Matrix ma2 = new Matrix(3, 3);
21670 calibrator.getInitialMa(ma2);
21671 assertEquals(ma1, ma2);
21672 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21673 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21674 final NEDPosition nedPosition1 = new NEDPosition();
21675 assertTrue(calibrator.getNedPosition(nedPosition1));
21676 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21677 assertNull(calibrator.getMeasurements());
21678 assertTrue(calibrator.isCommonAxisUsed());
21679 assertSame(calibrator.getListener(), this);
21680 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21681 assertFalse(calibrator.isReady());
21682 assertFalse(calibrator.isRunning());
21683 assertNull(calibrator.getEstimatedBiases());
21684 assertFalse(calibrator.getEstimatedBiases(null));
21685 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21686 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21687 assertNull(calibrator.getEstimatedBiasFx());
21688 assertNull(calibrator.getEstimatedBiasFy());
21689 assertNull(calibrator.getEstimatedBiasFz());
21690 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21691 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21692 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21693 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21694 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21695 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21696 assertNull(calibrator.getEstimatedMa());
21697 assertNull(calibrator.getEstimatedSx());
21698 assertNull(calibrator.getEstimatedSy());
21699 assertNull(calibrator.getEstimatedSz());
21700 assertNull(calibrator.getEstimatedMxy());
21701 assertNull(calibrator.getEstimatedMxz());
21702 assertNull(calibrator.getEstimatedMyx());
21703 assertNull(calibrator.getEstimatedMyz());
21704 assertNull(calibrator.getEstimatedMzx());
21705 assertNull(calibrator.getEstimatedMzy());
21706 assertNull(calibrator.getEstimatedCovariance());
21707 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21708 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21709 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21710 assertNotNull(calibrator.getGroundTruthGravityNorm());
21711 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21712 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21713 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21714 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21715 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21716 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21717 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21718 }
21719
21720 @Test
21721 public void testConstructor171() throws WrongSizeException {
21722 final Collection<StandardDeviationBodyKinematics> measurements =
21723 Collections.emptyList();
21724
21725 final Matrix ba = generateBa();
21726 final double biasX = ba.getElementAtIndex(0);
21727 final double biasY = ba.getElementAtIndex(1);
21728 final double biasZ = ba.getElementAtIndex(2);
21729
21730 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21731 final double latitude = Math.toRadians(
21732 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21733 final double longitude = Math.toRadians(
21734 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21735 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21736 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21737 final NEDVelocity nedVelocity = new NEDVelocity();
21738 final ECEFPosition ecefPosition = new ECEFPosition();
21739 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21740 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21741 ecefPosition, ecefVelocity);
21742
21743 final KnownPositionAccelerometerCalibrator calibrator =
21744 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21745 true, biasX, biasY, biasZ);
21746
21747
21748 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21749 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21750 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21751 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21752 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21753 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21754 final Acceleration bx2 = new Acceleration(0.0,
21755 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21756 calibrator.getInitialBiasXAsAcceleration(bx2);
21757 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21758 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21759 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21760 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21761 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21762 final Acceleration by2 = new Acceleration(0.0,
21763 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21764 calibrator.getInitialBiasYAsAcceleration(by2);
21765 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21766 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21767 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21768 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21769 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21770 final Acceleration bz2 = new Acceleration(0.0,
21771 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21772 calibrator.getInitialBiasZAsAcceleration(bz2);
21773 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21774 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21775 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21776 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21777 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21778 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21779 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21780 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21781 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21782 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21783 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21784 final double[] bias1 = calibrator.getInitialBias();
21785 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21786 final double[] bias2 = new double[3];
21787 calibrator.getInitialBias(bias2);
21788 assertArrayEquals(bias1, bias2, 0.0);
21789 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21790 assertEquals(b1, ba);
21791 final Matrix b2 = new Matrix(3, 1);
21792 calibrator.getInitialBiasAsMatrix(b2);
21793 assertEquals(b1, b2);
21794 final Matrix ma1 = calibrator.getInitialMa();
21795 assertEquals(ma1, new Matrix(3, 3));
21796 final Matrix ma2 = new Matrix(3, 3);
21797 calibrator.getInitialMa(ma2);
21798 assertEquals(ma1, ma2);
21799 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21800 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21801 final NEDPosition nedPosition1 = new NEDPosition();
21802 assertTrue(calibrator.getNedPosition(nedPosition1));
21803 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21804 assertSame(calibrator.getMeasurements(), measurements);
21805 assertTrue(calibrator.isCommonAxisUsed());
21806 assertNull(calibrator.getListener());
21807 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21808 assertFalse(calibrator.isReady());
21809 assertFalse(calibrator.isRunning());
21810 assertNull(calibrator.getEstimatedBiases());
21811 assertFalse(calibrator.getEstimatedBiases(null));
21812 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21813 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21814 assertNull(calibrator.getEstimatedBiasFx());
21815 assertNull(calibrator.getEstimatedBiasFy());
21816 assertNull(calibrator.getEstimatedBiasFz());
21817 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21818 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21819 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21820 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21821 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21822 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21823 assertNull(calibrator.getEstimatedMa());
21824 assertNull(calibrator.getEstimatedSx());
21825 assertNull(calibrator.getEstimatedSy());
21826 assertNull(calibrator.getEstimatedSz());
21827 assertNull(calibrator.getEstimatedMxy());
21828 assertNull(calibrator.getEstimatedMxz());
21829 assertNull(calibrator.getEstimatedMyx());
21830 assertNull(calibrator.getEstimatedMyz());
21831 assertNull(calibrator.getEstimatedMzx());
21832 assertNull(calibrator.getEstimatedMzy());
21833 assertNull(calibrator.getEstimatedCovariance());
21834 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21835 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21836 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21837 assertNotNull(calibrator.getGroundTruthGravityNorm());
21838 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21839 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21840 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21841 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21842 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21843 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21844 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21845 }
21846
21847 @Test
21848 public void testConstructor172() throws WrongSizeException {
21849 final Collection<StandardDeviationBodyKinematics> measurements =
21850 Collections.emptyList();
21851
21852 final Matrix ba = generateBa();
21853 final double biasX = ba.getElementAtIndex(0);
21854 final double biasY = ba.getElementAtIndex(1);
21855 final double biasZ = ba.getElementAtIndex(2);
21856
21857 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21858 final double latitude = Math.toRadians(
21859 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21860 final double longitude = Math.toRadians(
21861 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21862 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21863 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21864 final NEDVelocity nedVelocity = new NEDVelocity();
21865 final ECEFPosition ecefPosition = new ECEFPosition();
21866 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21867 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21868 ecefPosition, ecefVelocity);
21869
21870 final KnownPositionAccelerometerCalibrator calibrator =
21871 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
21872 true, biasX, biasY, biasZ, this);
21873
21874
21875 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
21876 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
21877 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
21878 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
21879 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
21880 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21881 final Acceleration bx2 = new Acceleration(0.0,
21882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21883 calibrator.getInitialBiasXAsAcceleration(bx2);
21884 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
21885 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21886 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
21887 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
21888 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21889 final Acceleration by2 = new Acceleration(0.0,
21890 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21891 calibrator.getInitialBiasYAsAcceleration(by2);
21892 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
21893 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21894 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
21895 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
21896 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21897 final Acceleration bz2 = new Acceleration(0.0,
21898 AccelerationUnit.FEET_PER_SQUARED_SECOND);
21899 calibrator.getInitialBiasZAsAcceleration(bz2);
21900 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
21901 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
21902 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
21903 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
21904 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
21905 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
21906 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
21907 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
21908 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
21909 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
21910 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
21911 final double[] bias1 = calibrator.getInitialBias();
21912 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
21913 final double[] bias2 = new double[3];
21914 calibrator.getInitialBias(bias2);
21915 assertArrayEquals(bias1, bias2, 0.0);
21916 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
21917 assertEquals(b1, ba);
21918 final Matrix b2 = new Matrix(3, 1);
21919 calibrator.getInitialBiasAsMatrix(b2);
21920 assertEquals(b1, b2);
21921 final Matrix ma1 = calibrator.getInitialMa();
21922 assertEquals(ma1, new Matrix(3, 3));
21923 final Matrix ma2 = new Matrix(3, 3);
21924 calibrator.getInitialMa(ma2);
21925 assertEquals(ma1, ma2);
21926 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
21927 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
21928 final NEDPosition nedPosition1 = new NEDPosition();
21929 assertTrue(calibrator.getNedPosition(nedPosition1));
21930 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
21931 assertSame(calibrator.getMeasurements(), measurements);
21932 assertTrue(calibrator.isCommonAxisUsed());
21933 assertSame(calibrator.getListener(), this);
21934 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
21935 assertFalse(calibrator.isReady());
21936 assertFalse(calibrator.isRunning());
21937 assertNull(calibrator.getEstimatedBiases());
21938 assertFalse(calibrator.getEstimatedBiases(null));
21939 assertNull(calibrator.getEstimatedBiasesAsMatrix());
21940 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
21941 assertNull(calibrator.getEstimatedBiasFx());
21942 assertNull(calibrator.getEstimatedBiasFy());
21943 assertNull(calibrator.getEstimatedBiasFz());
21944 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
21945 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
21946 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
21947 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
21948 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
21949 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
21950 assertNull(calibrator.getEstimatedMa());
21951 assertNull(calibrator.getEstimatedSx());
21952 assertNull(calibrator.getEstimatedSy());
21953 assertNull(calibrator.getEstimatedSz());
21954 assertNull(calibrator.getEstimatedMxy());
21955 assertNull(calibrator.getEstimatedMxz());
21956 assertNull(calibrator.getEstimatedMyx());
21957 assertNull(calibrator.getEstimatedMyz());
21958 assertNull(calibrator.getEstimatedMzx());
21959 assertNull(calibrator.getEstimatedMzy());
21960 assertNull(calibrator.getEstimatedCovariance());
21961 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
21962 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
21963 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
21964 assertNotNull(calibrator.getGroundTruthGravityNorm());
21965 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
21966 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
21967 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
21968 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
21969 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
21970 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
21971 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
21972 }
21973
21974 @Test
21975 public void testConstructor173() throws WrongSizeException {
21976 final Matrix ba = generateBa();
21977 final double biasX = ba.getElementAtIndex(0);
21978 final double biasY = ba.getElementAtIndex(1);
21979 final double biasZ = ba.getElementAtIndex(2);
21980
21981 final Acceleration bx = new Acceleration(biasX,
21982 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21983 final Acceleration by = new Acceleration(biasY,
21984 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21985 final Acceleration bz = new Acceleration(biasZ,
21986 AccelerationUnit.METERS_PER_SQUARED_SECOND);
21987
21988 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
21989 final double latitude = Math.toRadians(
21990 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
21991 final double longitude = Math.toRadians(
21992 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
21993 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
21994 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
21995 final NEDVelocity nedVelocity = new NEDVelocity();
21996 final ECEFPosition ecefPosition = new ECEFPosition();
21997 final ECEFVelocity ecefVelocity = new ECEFVelocity();
21998 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
21999 ecefPosition, ecefVelocity);
22000
22001 final KnownPositionAccelerometerCalibrator calibrator =
22002 new KnownPositionAccelerometerCalibrator(nedPosition,
22003 bx, by, bz);
22004
22005
22006 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22007 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22008 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22009 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22010 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22011 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22012 final Acceleration bx2 = new Acceleration(0.0,
22013 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22014 calibrator.getInitialBiasXAsAcceleration(bx2);
22015 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22016 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22017 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22018 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22019 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22020 final Acceleration by2 = new Acceleration(0.0,
22021 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22022 calibrator.getInitialBiasYAsAcceleration(by2);
22023 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22024 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22025 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22026 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22027 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22028 final Acceleration bz2 = new Acceleration(0.0,
22029 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22030 calibrator.getInitialBiasZAsAcceleration(bz2);
22031 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22032 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22033 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22034 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22035 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22036 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22037 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22038 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22039 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22040 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22041 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22042 final double[] bias1 = calibrator.getInitialBias();
22043 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22044 final double[] bias2 = new double[3];
22045 calibrator.getInitialBias(bias2);
22046 assertArrayEquals(bias1, bias2, 0.0);
22047 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22048 assertEquals(b1, ba);
22049 final Matrix b2 = new Matrix(3, 1);
22050 calibrator.getInitialBiasAsMatrix(b2);
22051 assertEquals(b1, b2);
22052 final Matrix ma1 = calibrator.getInitialMa();
22053 assertEquals(ma1, new Matrix(3, 3));
22054 final Matrix ma2 = new Matrix(3, 3);
22055 calibrator.getInitialMa(ma2);
22056 assertEquals(ma1, ma2);
22057 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22058 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22059 final NEDPosition nedPosition1 = new NEDPosition();
22060 assertTrue(calibrator.getNedPosition(nedPosition1));
22061 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22062 assertNull(calibrator.getMeasurements());
22063 assertFalse(calibrator.isCommonAxisUsed());
22064 assertNull(calibrator.getListener());
22065 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22066 assertFalse(calibrator.isReady());
22067 assertFalse(calibrator.isRunning());
22068 assertNull(calibrator.getEstimatedBiases());
22069 assertFalse(calibrator.getEstimatedBiases(null));
22070 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22071 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22072 assertNull(calibrator.getEstimatedBiasFx());
22073 assertNull(calibrator.getEstimatedBiasFy());
22074 assertNull(calibrator.getEstimatedBiasFz());
22075 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22076 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22077 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22078 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22079 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22080 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22081 assertNull(calibrator.getEstimatedMa());
22082 assertNull(calibrator.getEstimatedSx());
22083 assertNull(calibrator.getEstimatedSy());
22084 assertNull(calibrator.getEstimatedSz());
22085 assertNull(calibrator.getEstimatedMxy());
22086 assertNull(calibrator.getEstimatedMxz());
22087 assertNull(calibrator.getEstimatedMyx());
22088 assertNull(calibrator.getEstimatedMyz());
22089 assertNull(calibrator.getEstimatedMzx());
22090 assertNull(calibrator.getEstimatedMzy());
22091 assertNull(calibrator.getEstimatedCovariance());
22092 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22093 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22094 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22095 assertNotNull(calibrator.getGroundTruthGravityNorm());
22096 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22097 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22098 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22099 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22100 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22101 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22102 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22103 }
22104
22105 @Test
22106 public void testConstructor174() throws WrongSizeException {
22107 final Matrix ba = generateBa();
22108 final double biasX = ba.getElementAtIndex(0);
22109 final double biasY = ba.getElementAtIndex(1);
22110 final double biasZ = ba.getElementAtIndex(2);
22111
22112 final Acceleration bx = new Acceleration(biasX,
22113 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22114 final Acceleration by = new Acceleration(biasY,
22115 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22116 final Acceleration bz = new Acceleration(biasZ,
22117 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22118
22119 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22120 final double latitude = Math.toRadians(
22121 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22122 final double longitude = Math.toRadians(
22123 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22124 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22125 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22126 final NEDVelocity nedVelocity = new NEDVelocity();
22127 final ECEFPosition ecefPosition = new ECEFPosition();
22128 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22129 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22130 ecefPosition, ecefVelocity);
22131
22132 final KnownPositionAccelerometerCalibrator calibrator =
22133 new KnownPositionAccelerometerCalibrator(nedPosition,
22134 bx, by, bz, this);
22135
22136
22137 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22138 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22139 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22140 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22141 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22142 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22143 final Acceleration bx2 = new Acceleration(0.0,
22144 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22145 calibrator.getInitialBiasXAsAcceleration(bx2);
22146 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22147 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22148 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22149 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22150 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22151 final Acceleration by2 = new Acceleration(0.0,
22152 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22153 calibrator.getInitialBiasYAsAcceleration(by2);
22154 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22155 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22156 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22157 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22158 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22159 final Acceleration bz2 = new Acceleration(0.0,
22160 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22161 calibrator.getInitialBiasZAsAcceleration(bz2);
22162 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22163 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22164 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22165 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22166 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22167 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22168 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22169 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22170 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22171 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22172 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22173 final double[] bias1 = calibrator.getInitialBias();
22174 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22175 final double[] bias2 = new double[3];
22176 calibrator.getInitialBias(bias2);
22177 assertArrayEquals(bias1, bias2, 0.0);
22178 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22179 assertEquals(b1, ba);
22180 final Matrix b2 = new Matrix(3, 1);
22181 calibrator.getInitialBiasAsMatrix(b2);
22182 assertEquals(b1, b2);
22183 final Matrix ma1 = calibrator.getInitialMa();
22184 assertEquals(ma1, new Matrix(3, 3));
22185 final Matrix ma2 = new Matrix(3, 3);
22186 calibrator.getInitialMa(ma2);
22187 assertEquals(ma1, ma2);
22188 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22189 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22190 final NEDPosition nedPosition1 = new NEDPosition();
22191 assertTrue(calibrator.getNedPosition(nedPosition1));
22192 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22193 assertNull(calibrator.getMeasurements());
22194 assertFalse(calibrator.isCommonAxisUsed());
22195 assertSame(calibrator.getListener(), this);
22196 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22197 assertFalse(calibrator.isReady());
22198 assertFalse(calibrator.isRunning());
22199 assertNull(calibrator.getEstimatedBiases());
22200 assertFalse(calibrator.getEstimatedBiases(null));
22201 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22202 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22203 assertNull(calibrator.getEstimatedBiasFx());
22204 assertNull(calibrator.getEstimatedBiasFy());
22205 assertNull(calibrator.getEstimatedBiasFz());
22206 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22207 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22208 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22209 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22210 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22211 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22212 assertNull(calibrator.getEstimatedMa());
22213 assertNull(calibrator.getEstimatedSx());
22214 assertNull(calibrator.getEstimatedSy());
22215 assertNull(calibrator.getEstimatedSz());
22216 assertNull(calibrator.getEstimatedMxy());
22217 assertNull(calibrator.getEstimatedMxz());
22218 assertNull(calibrator.getEstimatedMyx());
22219 assertNull(calibrator.getEstimatedMyz());
22220 assertNull(calibrator.getEstimatedMzx());
22221 assertNull(calibrator.getEstimatedMzy());
22222 assertNull(calibrator.getEstimatedCovariance());
22223 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22224 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22225 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22226 assertNotNull(calibrator.getGroundTruthGravityNorm());
22227 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22228 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22229 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22230 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22231 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22232 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22233 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22234 }
22235
22236 @Test
22237 public void testConstructor175() throws WrongSizeException {
22238 final Collection<StandardDeviationBodyKinematics> measurements =
22239 Collections.emptyList();
22240
22241 final Matrix ba = generateBa();
22242 final double biasX = ba.getElementAtIndex(0);
22243 final double biasY = ba.getElementAtIndex(1);
22244 final double biasZ = ba.getElementAtIndex(2);
22245
22246 final Acceleration bx = new Acceleration(biasX,
22247 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22248 final Acceleration by = new Acceleration(biasY,
22249 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22250 final Acceleration bz = new Acceleration(biasZ,
22251 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22252
22253 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22254 final double latitude = Math.toRadians(
22255 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22256 final double longitude = Math.toRadians(
22257 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22258 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22259 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22260 final NEDVelocity nedVelocity = new NEDVelocity();
22261 final ECEFPosition ecefPosition = new ECEFPosition();
22262 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22263 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22264 ecefPosition, ecefVelocity);
22265
22266 final KnownPositionAccelerometerCalibrator calibrator =
22267 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22268 bx, by, bz);
22269
22270
22271 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22272 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22273 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22274 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22275 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22276 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22277 final Acceleration bx2 = new Acceleration(0.0,
22278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22279 calibrator.getInitialBiasXAsAcceleration(bx2);
22280 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22281 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22282 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22283 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22284 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22285 final Acceleration by2 = new Acceleration(0.0,
22286 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22287 calibrator.getInitialBiasYAsAcceleration(by2);
22288 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22289 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22290 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22291 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22292 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22293 final Acceleration bz2 = new Acceleration(0.0,
22294 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22295 calibrator.getInitialBiasZAsAcceleration(bz2);
22296 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22297 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22298 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22299 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22300 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22301 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22302 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22303 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22304 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22305 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22306 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22307 final double[] bias1 = calibrator.getInitialBias();
22308 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22309 final double[] bias2 = new double[3];
22310 calibrator.getInitialBias(bias2);
22311 assertArrayEquals(bias1, bias2, 0.0);
22312 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22313 assertEquals(b1, ba);
22314 final Matrix b2 = new Matrix(3, 1);
22315 calibrator.getInitialBiasAsMatrix(b2);
22316 assertEquals(b1, b2);
22317 final Matrix ma1 = calibrator.getInitialMa();
22318 assertEquals(ma1, new Matrix(3, 3));
22319 final Matrix ma2 = new Matrix(3, 3);
22320 calibrator.getInitialMa(ma2);
22321 assertEquals(ma1, ma2);
22322 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22323 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22324 final NEDPosition nedPosition1 = new NEDPosition();
22325 assertTrue(calibrator.getNedPosition(nedPosition1));
22326 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22327 assertSame(calibrator.getMeasurements(), measurements);
22328 assertFalse(calibrator.isCommonAxisUsed());
22329 assertNull(calibrator.getListener());
22330 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22331 assertFalse(calibrator.isReady());
22332 assertFalse(calibrator.isRunning());
22333 assertNull(calibrator.getEstimatedBiases());
22334 assertFalse(calibrator.getEstimatedBiases(null));
22335 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22336 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22337 assertNull(calibrator.getEstimatedBiasFx());
22338 assertNull(calibrator.getEstimatedBiasFy());
22339 assertNull(calibrator.getEstimatedBiasFz());
22340 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22341 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22342 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22343 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22344 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22345 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22346 assertNull(calibrator.getEstimatedMa());
22347 assertNull(calibrator.getEstimatedSx());
22348 assertNull(calibrator.getEstimatedSy());
22349 assertNull(calibrator.getEstimatedSz());
22350 assertNull(calibrator.getEstimatedMxy());
22351 assertNull(calibrator.getEstimatedMxz());
22352 assertNull(calibrator.getEstimatedMyx());
22353 assertNull(calibrator.getEstimatedMyz());
22354 assertNull(calibrator.getEstimatedMzx());
22355 assertNull(calibrator.getEstimatedMzy());
22356 assertNull(calibrator.getEstimatedCovariance());
22357 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22358 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22359 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22360 assertNotNull(calibrator.getGroundTruthGravityNorm());
22361 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22362 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22363 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22364 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22365 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22366 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22367 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22368 }
22369
22370 @Test
22371 public void testConstructor176() throws WrongSizeException {
22372 final Collection<StandardDeviationBodyKinematics> measurements =
22373 Collections.emptyList();
22374
22375 final Matrix ba = generateBa();
22376 final double biasX = ba.getElementAtIndex(0);
22377 final double biasY = ba.getElementAtIndex(1);
22378 final double biasZ = ba.getElementAtIndex(2);
22379
22380 final Acceleration bx = new Acceleration(biasX,
22381 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22382 final Acceleration by = new Acceleration(biasY,
22383 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22384 final Acceleration bz = new Acceleration(biasZ,
22385 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22386
22387 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22388 final double latitude = Math.toRadians(
22389 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22390 final double longitude = Math.toRadians(
22391 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22392 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22393 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22394 final NEDVelocity nedVelocity = new NEDVelocity();
22395 final ECEFPosition ecefPosition = new ECEFPosition();
22396 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22397 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22398 ecefPosition, ecefVelocity);
22399
22400 final KnownPositionAccelerometerCalibrator calibrator =
22401 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22402 bx, by, bz, this);
22403
22404
22405 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22406 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22407 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22408 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22409 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22410 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22411 final Acceleration bx2 = new Acceleration(0.0,
22412 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22413 calibrator.getInitialBiasXAsAcceleration(bx2);
22414 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22415 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22416 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22417 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22418 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22419 final Acceleration by2 = new Acceleration(0.0,
22420 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22421 calibrator.getInitialBiasYAsAcceleration(by2);
22422 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22423 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22424 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22425 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22426 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22427 final Acceleration bz2 = new Acceleration(0.0,
22428 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22429 calibrator.getInitialBiasZAsAcceleration(bz2);
22430 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22431 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22432 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22433 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22434 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22435 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22436 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22437 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22438 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22439 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22440 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22441 final double[] bias1 = calibrator.getInitialBias();
22442 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22443 final double[] bias2 = new double[3];
22444 calibrator.getInitialBias(bias2);
22445 assertArrayEquals(bias1, bias2, 0.0);
22446 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22447 assertEquals(b1, ba);
22448 final Matrix b2 = new Matrix(3, 1);
22449 calibrator.getInitialBiasAsMatrix(b2);
22450 assertEquals(b1, b2);
22451 final Matrix ma1 = calibrator.getInitialMa();
22452 assertEquals(ma1, new Matrix(3, 3));
22453 final Matrix ma2 = new Matrix(3, 3);
22454 calibrator.getInitialMa(ma2);
22455 assertEquals(ma1, ma2);
22456 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22457 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22458 final NEDPosition nedPosition1 = new NEDPosition();
22459 assertTrue(calibrator.getNedPosition(nedPosition1));
22460 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22461 assertSame(calibrator.getMeasurements(), measurements);
22462 assertFalse(calibrator.isCommonAxisUsed());
22463 assertSame(calibrator.getListener(), this);
22464 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
22465 assertFalse(calibrator.isReady());
22466 assertFalse(calibrator.isRunning());
22467 assertNull(calibrator.getEstimatedBiases());
22468 assertFalse(calibrator.getEstimatedBiases(null));
22469 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22470 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22471 assertNull(calibrator.getEstimatedBiasFx());
22472 assertNull(calibrator.getEstimatedBiasFy());
22473 assertNull(calibrator.getEstimatedBiasFz());
22474 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22475 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22476 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22477 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22478 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22479 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22480 assertNull(calibrator.getEstimatedMa());
22481 assertNull(calibrator.getEstimatedSx());
22482 assertNull(calibrator.getEstimatedSy());
22483 assertNull(calibrator.getEstimatedSz());
22484 assertNull(calibrator.getEstimatedMxy());
22485 assertNull(calibrator.getEstimatedMxz());
22486 assertNull(calibrator.getEstimatedMyx());
22487 assertNull(calibrator.getEstimatedMyz());
22488 assertNull(calibrator.getEstimatedMzx());
22489 assertNull(calibrator.getEstimatedMzy());
22490 assertNull(calibrator.getEstimatedCovariance());
22491 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22492 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22493 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22494 assertNotNull(calibrator.getGroundTruthGravityNorm());
22495 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22496 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22497 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22498 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22499 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22500 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22501 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22502 }
22503
22504 @Test
22505 public void testConstructor177() throws WrongSizeException {
22506 final Matrix ba = generateBa();
22507 final double biasX = ba.getElementAtIndex(0);
22508 final double biasY = ba.getElementAtIndex(1);
22509 final double biasZ = ba.getElementAtIndex(2);
22510
22511 final Acceleration bx = new Acceleration(biasX,
22512 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22513 final Acceleration by = new Acceleration(biasY,
22514 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22515 final Acceleration bz = new Acceleration(biasZ,
22516 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22517
22518 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22519 final double latitude = Math.toRadians(
22520 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22521 final double longitude = Math.toRadians(
22522 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22523 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22524 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22525 final NEDVelocity nedVelocity = new NEDVelocity();
22526 final ECEFPosition ecefPosition = new ECEFPosition();
22527 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22528 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22529 ecefPosition, ecefVelocity);
22530
22531 final KnownPositionAccelerometerCalibrator calibrator =
22532 new KnownPositionAccelerometerCalibrator(nedPosition,
22533 true, bx, by, bz);
22534
22535
22536 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22537 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22538 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22539 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22540 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22541 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22542 final Acceleration bx2 = new Acceleration(0.0,
22543 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22544 calibrator.getInitialBiasXAsAcceleration(bx2);
22545 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22546 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22547 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22548 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22549 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22550 final Acceleration by2 = new Acceleration(0.0,
22551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22552 calibrator.getInitialBiasYAsAcceleration(by2);
22553 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22554 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22555 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22556 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22557 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22558 final Acceleration bz2 = new Acceleration(0.0,
22559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22560 calibrator.getInitialBiasZAsAcceleration(bz2);
22561 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22562 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22563 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22564 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22565 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22566 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22567 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22568 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22569 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22570 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22571 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22572 final double[] bias1 = calibrator.getInitialBias();
22573 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22574 final double[] bias2 = new double[3];
22575 calibrator.getInitialBias(bias2);
22576 assertArrayEquals(bias1, bias2, 0.0);
22577 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22578 assertEquals(b1, ba);
22579 final Matrix b2 = new Matrix(3, 1);
22580 calibrator.getInitialBiasAsMatrix(b2);
22581 assertEquals(b1, b2);
22582 final Matrix ma1 = calibrator.getInitialMa();
22583 assertEquals(ma1, new Matrix(3, 3));
22584 final Matrix ma2 = new Matrix(3, 3);
22585 calibrator.getInitialMa(ma2);
22586 assertEquals(ma1, ma2);
22587 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22588 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22589 final NEDPosition nedPosition1 = new NEDPosition();
22590 assertTrue(calibrator.getNedPosition(nedPosition1));
22591 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22592 assertNull(calibrator.getMeasurements());
22593 assertTrue(calibrator.isCommonAxisUsed());
22594 assertNull(calibrator.getListener());
22595 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22596 assertFalse(calibrator.isReady());
22597 assertFalse(calibrator.isRunning());
22598 assertNull(calibrator.getEstimatedBiases());
22599 assertFalse(calibrator.getEstimatedBiases(null));
22600 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22601 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22602 assertNull(calibrator.getEstimatedBiasFx());
22603 assertNull(calibrator.getEstimatedBiasFy());
22604 assertNull(calibrator.getEstimatedBiasFz());
22605 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22606 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22607 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22608 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22609 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22610 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22611 assertNull(calibrator.getEstimatedMa());
22612 assertNull(calibrator.getEstimatedSx());
22613 assertNull(calibrator.getEstimatedSy());
22614 assertNull(calibrator.getEstimatedSz());
22615 assertNull(calibrator.getEstimatedMxy());
22616 assertNull(calibrator.getEstimatedMxz());
22617 assertNull(calibrator.getEstimatedMyx());
22618 assertNull(calibrator.getEstimatedMyz());
22619 assertNull(calibrator.getEstimatedMzx());
22620 assertNull(calibrator.getEstimatedMzy());
22621 assertNull(calibrator.getEstimatedCovariance());
22622 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22623 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22624 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22625 assertNotNull(calibrator.getGroundTruthGravityNorm());
22626 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22627 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22628 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22629 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22630 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22631 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22632 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22633 }
22634
22635 @Test
22636 public void testConstructor178() throws WrongSizeException {
22637 final Matrix ba = generateBa();
22638 final double biasX = ba.getElementAtIndex(0);
22639 final double biasY = ba.getElementAtIndex(1);
22640 final double biasZ = ba.getElementAtIndex(2);
22641
22642 final Acceleration bx = new Acceleration(biasX,
22643 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22644 final Acceleration by = new Acceleration(biasY,
22645 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22646 final Acceleration bz = new Acceleration(biasZ,
22647 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22648
22649 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22650 final double latitude = Math.toRadians(
22651 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22652 final double longitude = Math.toRadians(
22653 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22654 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22655 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22656 final NEDVelocity nedVelocity = new NEDVelocity();
22657 final ECEFPosition ecefPosition = new ECEFPosition();
22658 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22659 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22660 ecefPosition, ecefVelocity);
22661
22662 final KnownPositionAccelerometerCalibrator calibrator =
22663 new KnownPositionAccelerometerCalibrator(nedPosition,
22664 true, bx, by, bz, this);
22665
22666
22667 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22668 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22669 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22670 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22671 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22672 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22673 final Acceleration bx2 = new Acceleration(0.0,
22674 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22675 calibrator.getInitialBiasXAsAcceleration(bx2);
22676 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22677 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22678 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22679 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22680 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22681 final Acceleration by2 = new Acceleration(0.0,
22682 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22683 calibrator.getInitialBiasYAsAcceleration(by2);
22684 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22685 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22686 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22687 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22688 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22689 final Acceleration bz2 = new Acceleration(0.0,
22690 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22691 calibrator.getInitialBiasZAsAcceleration(bz2);
22692 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22693 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22694 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22695 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22696 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22697 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22698 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22699 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22700 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22701 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22702 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22703 final double[] bias1 = calibrator.getInitialBias();
22704 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22705 final double[] bias2 = new double[3];
22706 calibrator.getInitialBias(bias2);
22707 assertArrayEquals(bias1, bias2, 0.0);
22708 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22709 assertEquals(b1, ba);
22710 final Matrix b2 = new Matrix(3, 1);
22711 calibrator.getInitialBiasAsMatrix(b2);
22712 assertEquals(b1, b2);
22713 final Matrix ma1 = calibrator.getInitialMa();
22714 assertEquals(ma1, new Matrix(3, 3));
22715 final Matrix ma2 = new Matrix(3, 3);
22716 calibrator.getInitialMa(ma2);
22717 assertEquals(ma1, ma2);
22718 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22719 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22720 final NEDPosition nedPosition1 = new NEDPosition();
22721 assertTrue(calibrator.getNedPosition(nedPosition1));
22722 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22723 assertNull(calibrator.getMeasurements());
22724 assertTrue(calibrator.isCommonAxisUsed());
22725 assertSame(calibrator.getListener(), this);
22726 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22727 assertFalse(calibrator.isReady());
22728 assertFalse(calibrator.isRunning());
22729 assertNull(calibrator.getEstimatedBiases());
22730 assertFalse(calibrator.getEstimatedBiases(null));
22731 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22732 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22733 assertNull(calibrator.getEstimatedBiasFx());
22734 assertNull(calibrator.getEstimatedBiasFy());
22735 assertNull(calibrator.getEstimatedBiasFz());
22736 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22737 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22738 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22739 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22740 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22741 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22742 assertNull(calibrator.getEstimatedMa());
22743 assertNull(calibrator.getEstimatedSx());
22744 assertNull(calibrator.getEstimatedSy());
22745 assertNull(calibrator.getEstimatedSz());
22746 assertNull(calibrator.getEstimatedMxy());
22747 assertNull(calibrator.getEstimatedMxz());
22748 assertNull(calibrator.getEstimatedMyx());
22749 assertNull(calibrator.getEstimatedMyz());
22750 assertNull(calibrator.getEstimatedMzx());
22751 assertNull(calibrator.getEstimatedMzy());
22752 assertNull(calibrator.getEstimatedCovariance());
22753 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22754 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22755 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22756 assertNotNull(calibrator.getGroundTruthGravityNorm());
22757 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22758 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22759 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22760 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22761 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22762 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22763 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22764 }
22765
22766 @Test
22767 public void testConstructor179() throws WrongSizeException {
22768 final Collection<StandardDeviationBodyKinematics> measurements =
22769 Collections.emptyList();
22770
22771 final Matrix ba = generateBa();
22772 final double biasX = ba.getElementAtIndex(0);
22773 final double biasY = ba.getElementAtIndex(1);
22774 final double biasZ = ba.getElementAtIndex(2);
22775
22776 final Acceleration bx = new Acceleration(biasX,
22777 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22778 final Acceleration by = new Acceleration(biasY,
22779 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22780 final Acceleration bz = new Acceleration(biasZ,
22781 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22782
22783 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22784 final double latitude = Math.toRadians(
22785 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22786 final double longitude = Math.toRadians(
22787 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22788 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22789 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22790 final NEDVelocity nedVelocity = new NEDVelocity();
22791 final ECEFPosition ecefPosition = new ECEFPosition();
22792 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22793 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22794 ecefPosition, ecefVelocity);
22795
22796 final KnownPositionAccelerometerCalibrator calibrator =
22797 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22798 true, bx, by, bz);
22799
22800
22801 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22802 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22803 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22804 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22805 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22806 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22807 final Acceleration bx2 = new Acceleration(0.0,
22808 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22809 calibrator.getInitialBiasXAsAcceleration(bx2);
22810 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22811 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22812 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22813 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22814 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22815 final Acceleration by2 = new Acceleration(0.0,
22816 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22817 calibrator.getInitialBiasYAsAcceleration(by2);
22818 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22819 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22820 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22821 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22822 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22823 final Acceleration bz2 = new Acceleration(0.0,
22824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22825 calibrator.getInitialBiasZAsAcceleration(bz2);
22826 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22827 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22828 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22829 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22830 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22831 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22832 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22833 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22834 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22835 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22836 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22837 final double[] bias1 = calibrator.getInitialBias();
22838 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22839 final double[] bias2 = new double[3];
22840 calibrator.getInitialBias(bias2);
22841 assertArrayEquals(bias1, bias2, 0.0);
22842 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22843 assertEquals(b1, ba);
22844 final Matrix b2 = new Matrix(3, 1);
22845 calibrator.getInitialBiasAsMatrix(b2);
22846 assertEquals(b1, b2);
22847 final Matrix ma1 = calibrator.getInitialMa();
22848 assertEquals(ma1, new Matrix(3, 3));
22849 final Matrix ma2 = new Matrix(3, 3);
22850 calibrator.getInitialMa(ma2);
22851 assertEquals(ma1, ma2);
22852 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22853 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22854 final NEDPosition nedPosition1 = new NEDPosition();
22855 assertTrue(calibrator.getNedPosition(nedPosition1));
22856 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22857 assertSame(calibrator.getMeasurements(), measurements);
22858 assertTrue(calibrator.isCommonAxisUsed());
22859 assertNull(calibrator.getListener());
22860 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22861 assertFalse(calibrator.isReady());
22862 assertFalse(calibrator.isRunning());
22863 assertNull(calibrator.getEstimatedBiases());
22864 assertFalse(calibrator.getEstimatedBiases(null));
22865 assertNull(calibrator.getEstimatedBiasesAsMatrix());
22866 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
22867 assertNull(calibrator.getEstimatedBiasFx());
22868 assertNull(calibrator.getEstimatedBiasFy());
22869 assertNull(calibrator.getEstimatedBiasFz());
22870 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
22871 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
22872 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
22873 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
22874 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
22875 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
22876 assertNull(calibrator.getEstimatedMa());
22877 assertNull(calibrator.getEstimatedSx());
22878 assertNull(calibrator.getEstimatedSy());
22879 assertNull(calibrator.getEstimatedSz());
22880 assertNull(calibrator.getEstimatedMxy());
22881 assertNull(calibrator.getEstimatedMxz());
22882 assertNull(calibrator.getEstimatedMyx());
22883 assertNull(calibrator.getEstimatedMyz());
22884 assertNull(calibrator.getEstimatedMzx());
22885 assertNull(calibrator.getEstimatedMzy());
22886 assertNull(calibrator.getEstimatedCovariance());
22887 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
22888 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
22889 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
22890 assertNotNull(calibrator.getGroundTruthGravityNorm());
22891 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
22892 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
22893 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
22894 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
22895 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
22896 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
22897 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
22898 }
22899
22900 @Test
22901 public void testConstructor180() throws WrongSizeException {
22902 final Collection<StandardDeviationBodyKinematics> measurements =
22903 Collections.emptyList();
22904
22905 final Matrix ba = generateBa();
22906 final double biasX = ba.getElementAtIndex(0);
22907 final double biasY = ba.getElementAtIndex(1);
22908 final double biasZ = ba.getElementAtIndex(2);
22909
22910 final Acceleration bx = new Acceleration(biasX,
22911 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22912 final Acceleration by = new Acceleration(biasY,
22913 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22914 final Acceleration bz = new Acceleration(biasZ,
22915 AccelerationUnit.METERS_PER_SQUARED_SECOND);
22916
22917 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
22918 final double latitude = Math.toRadians(
22919 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
22920 final double longitude = Math.toRadians(
22921 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
22922 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
22923 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
22924 final NEDVelocity nedVelocity = new NEDVelocity();
22925 final ECEFPosition ecefPosition = new ECEFPosition();
22926 final ECEFVelocity ecefVelocity = new ECEFVelocity();
22927 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
22928 ecefPosition, ecefVelocity);
22929
22930 final KnownPositionAccelerometerCalibrator calibrator =
22931 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
22932 true, bx, by, bz, this);
22933
22934
22935 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
22936 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
22937 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
22938 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
22939 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
22940 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22941 final Acceleration bx2 = new Acceleration(0.0,
22942 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22943 calibrator.getInitialBiasXAsAcceleration(bx2);
22944 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
22945 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22946 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
22947 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
22948 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22949 final Acceleration by2 = new Acceleration(0.0,
22950 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22951 calibrator.getInitialBiasYAsAcceleration(by2);
22952 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
22953 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22954 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
22955 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
22956 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22957 final Acceleration bz2 = new Acceleration(0.0,
22958 AccelerationUnit.FEET_PER_SQUARED_SECOND);
22959 calibrator.getInitialBiasZAsAcceleration(bz2);
22960 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
22961 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
22962 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
22963 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
22964 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
22965 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
22966 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
22967 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
22968 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
22969 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
22970 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
22971 final double[] bias1 = calibrator.getInitialBias();
22972 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
22973 final double[] bias2 = new double[3];
22974 calibrator.getInitialBias(bias2);
22975 assertArrayEquals(bias1, bias2, 0.0);
22976 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
22977 assertEquals(b1, ba);
22978 final Matrix b2 = new Matrix(3, 1);
22979 calibrator.getInitialBiasAsMatrix(b2);
22980 assertEquals(b1, b2);
22981 final Matrix ma1 = calibrator.getInitialMa();
22982 assertEquals(ma1, new Matrix(3, 3));
22983 final Matrix ma2 = new Matrix(3, 3);
22984 calibrator.getInitialMa(ma2);
22985 assertEquals(ma1, ma2);
22986 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
22987 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
22988 final NEDPosition nedPosition1 = new NEDPosition();
22989 assertTrue(calibrator.getNedPosition(nedPosition1));
22990 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
22991 assertSame(calibrator.getMeasurements(), measurements);
22992 assertTrue(calibrator.isCommonAxisUsed());
22993 assertSame(calibrator.getListener(), this);
22994 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
22995 assertFalse(calibrator.isReady());
22996 assertFalse(calibrator.isRunning());
22997 assertNull(calibrator.getEstimatedBiases());
22998 assertFalse(calibrator.getEstimatedBiases(null));
22999 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23000 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23001 assertNull(calibrator.getEstimatedBiasFx());
23002 assertNull(calibrator.getEstimatedBiasFy());
23003 assertNull(calibrator.getEstimatedBiasFz());
23004 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23005 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23006 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23007 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23008 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23009 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23010 assertNull(calibrator.getEstimatedMa());
23011 assertNull(calibrator.getEstimatedSx());
23012 assertNull(calibrator.getEstimatedSy());
23013 assertNull(calibrator.getEstimatedSz());
23014 assertNull(calibrator.getEstimatedMxy());
23015 assertNull(calibrator.getEstimatedMxz());
23016 assertNull(calibrator.getEstimatedMyx());
23017 assertNull(calibrator.getEstimatedMyz());
23018 assertNull(calibrator.getEstimatedMzx());
23019 assertNull(calibrator.getEstimatedMzy());
23020 assertNull(calibrator.getEstimatedCovariance());
23021 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23022 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23023 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23024 assertNotNull(calibrator.getGroundTruthGravityNorm());
23025 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23026 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23027 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23028 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23029 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23030 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23031 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23032 }
23033
23034 @Test
23035 public void testConstructor181() throws WrongSizeException {
23036 final Matrix ba = generateBa();
23037 final double biasX = ba.getElementAtIndex(0);
23038 final double biasY = ba.getElementAtIndex(1);
23039 final double biasZ = ba.getElementAtIndex(2);
23040
23041 final Matrix ma = generateMaCommonAxis();
23042 final double sx = ma.getElementAt(0, 0);
23043 final double sy = ma.getElementAt(1, 1);
23044 final double sz = ma.getElementAt(2, 2);
23045
23046 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23047 final double latitude = Math.toRadians(
23048 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23049 final double longitude = Math.toRadians(
23050 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23051 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23052 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23053 final NEDVelocity nedVelocity = new NEDVelocity();
23054 final ECEFPosition ecefPosition = new ECEFPosition();
23055 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23056 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23057 ecefPosition, ecefVelocity);
23058
23059 final KnownPositionAccelerometerCalibrator calibrator =
23060 new KnownPositionAccelerometerCalibrator(nedPosition,
23061 biasX, biasY, biasZ, sx, sy, sz);
23062
23063
23064 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23065 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23066 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23067 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23068 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23069 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23070 final Acceleration bx2 = new Acceleration(0.0,
23071 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23072 calibrator.getInitialBiasXAsAcceleration(bx2);
23073 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23074 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23075 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23076 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23077 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23078 final Acceleration by2 = new Acceleration(0.0,
23079 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23080 calibrator.getInitialBiasYAsAcceleration(by2);
23081 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23082 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23083 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23084 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23085 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23086 final Acceleration bz2 = new Acceleration(0.0,
23087 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23088 calibrator.getInitialBiasZAsAcceleration(bz2);
23089 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23090 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23091 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23092 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23093 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23094 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23095 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23096 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23097 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23098 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23099 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23100 final double[] bias1 = calibrator.getInitialBias();
23101 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23102 final double[] bias2 = new double[3];
23103 calibrator.getInitialBias(bias2);
23104 assertArrayEquals(bias1, bias2, 0.0);
23105 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23106 assertEquals(b1, ba);
23107 final Matrix b2 = new Matrix(3, 1);
23108 calibrator.getInitialBiasAsMatrix(b2);
23109 assertEquals(b1, b2);
23110 final Matrix ma1 = calibrator.getInitialMa();
23111 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23112 final Matrix ma2 = new Matrix(3, 3);
23113 calibrator.getInitialMa(ma2);
23114 assertEquals(ma1, ma2);
23115 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23116 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23117 final NEDPosition nedPosition1 = new NEDPosition();
23118 assertTrue(calibrator.getNedPosition(nedPosition1));
23119 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23120 assertNull(calibrator.getMeasurements());
23121 assertFalse(calibrator.isCommonAxisUsed());
23122 assertNull(calibrator.getListener());
23123 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23124 assertFalse(calibrator.isReady());
23125 assertFalse(calibrator.isRunning());
23126 assertNull(calibrator.getEstimatedBiases());
23127 assertFalse(calibrator.getEstimatedBiases(null));
23128 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23129 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23130 assertNull(calibrator.getEstimatedBiasFx());
23131 assertNull(calibrator.getEstimatedBiasFy());
23132 assertNull(calibrator.getEstimatedBiasFz());
23133 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23134 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23135 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23136 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23137 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23138 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23139 assertNull(calibrator.getEstimatedMa());
23140 assertNull(calibrator.getEstimatedSx());
23141 assertNull(calibrator.getEstimatedSy());
23142 assertNull(calibrator.getEstimatedSz());
23143 assertNull(calibrator.getEstimatedMxy());
23144 assertNull(calibrator.getEstimatedMxz());
23145 assertNull(calibrator.getEstimatedMyx());
23146 assertNull(calibrator.getEstimatedMyz());
23147 assertNull(calibrator.getEstimatedMzx());
23148 assertNull(calibrator.getEstimatedMzy());
23149 assertNull(calibrator.getEstimatedCovariance());
23150 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23151 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23152 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23153 assertNotNull(calibrator.getGroundTruthGravityNorm());
23154 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23155 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23156 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23157 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23158 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23159 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23160 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23161 }
23162
23163 @Test
23164 public void testConstructor182() throws WrongSizeException {
23165 final Collection<StandardDeviationBodyKinematics> measurements =
23166 Collections.emptyList();
23167
23168 final Matrix ba = generateBa();
23169 final double biasX = ba.getElementAtIndex(0);
23170 final double biasY = ba.getElementAtIndex(1);
23171 final double biasZ = ba.getElementAtIndex(2);
23172
23173 final Matrix ma = generateMaCommonAxis();
23174 final double sx = ma.getElementAt(0, 0);
23175 final double sy = ma.getElementAt(1, 1);
23176 final double sz = ma.getElementAt(2, 2);
23177
23178 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23179 final double latitude = Math.toRadians(
23180 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23181 final double longitude = Math.toRadians(
23182 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23183 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23184 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23185 final NEDVelocity nedVelocity = new NEDVelocity();
23186 final ECEFPosition ecefPosition = new ECEFPosition();
23187 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23188 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23189 ecefPosition, ecefVelocity);
23190
23191 final KnownPositionAccelerometerCalibrator calibrator =
23192 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23193 biasX, biasY, biasZ, sx, sy, sz);
23194
23195
23196 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23197 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23198 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23199 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23200 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23201 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23202 final Acceleration bx2 = new Acceleration(0.0,
23203 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23204 calibrator.getInitialBiasXAsAcceleration(bx2);
23205 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23206 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23207 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23208 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23209 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23210 final Acceleration by2 = new Acceleration(0.0,
23211 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23212 calibrator.getInitialBiasYAsAcceleration(by2);
23213 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23214 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23215 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23216 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23217 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23218 final Acceleration bz2 = new Acceleration(0.0,
23219 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23220 calibrator.getInitialBiasZAsAcceleration(bz2);
23221 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23222 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23223 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23224 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23225 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23226 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23227 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23228 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23229 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23230 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23231 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23232 final double[] bias1 = calibrator.getInitialBias();
23233 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23234 final double[] bias2 = new double[3];
23235 calibrator.getInitialBias(bias2);
23236 assertArrayEquals(bias1, bias2, 0.0);
23237 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23238 assertEquals(b1, ba);
23239 final Matrix b2 = new Matrix(3, 1);
23240 calibrator.getInitialBiasAsMatrix(b2);
23241 assertEquals(b1, b2);
23242 final Matrix ma1 = calibrator.getInitialMa();
23243 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23244 final Matrix ma2 = new Matrix(3, 3);
23245 calibrator.getInitialMa(ma2);
23246 assertEquals(ma1, ma2);
23247 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23248 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23249 final NEDPosition nedPosition1 = new NEDPosition();
23250 assertTrue(calibrator.getNedPosition(nedPosition1));
23251 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23252 assertSame(calibrator.getMeasurements(), measurements);
23253 assertFalse(calibrator.isCommonAxisUsed());
23254 assertNull(calibrator.getListener());
23255 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23256 assertFalse(calibrator.isReady());
23257 assertFalse(calibrator.isRunning());
23258 assertNull(calibrator.getEstimatedBiases());
23259 assertFalse(calibrator.getEstimatedBiases(null));
23260 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23261 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23262 assertNull(calibrator.getEstimatedBiasFx());
23263 assertNull(calibrator.getEstimatedBiasFy());
23264 assertNull(calibrator.getEstimatedBiasFz());
23265 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23266 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23267 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23268 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23269 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23270 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23271 assertNull(calibrator.getEstimatedMa());
23272 assertNull(calibrator.getEstimatedSx());
23273 assertNull(calibrator.getEstimatedSy());
23274 assertNull(calibrator.getEstimatedSz());
23275 assertNull(calibrator.getEstimatedMxy());
23276 assertNull(calibrator.getEstimatedMxz());
23277 assertNull(calibrator.getEstimatedMyx());
23278 assertNull(calibrator.getEstimatedMyz());
23279 assertNull(calibrator.getEstimatedMzx());
23280 assertNull(calibrator.getEstimatedMzy());
23281 assertNull(calibrator.getEstimatedCovariance());
23282 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23283 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23284 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23285 assertNotNull(calibrator.getGroundTruthGravityNorm());
23286 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23287 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23288 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23289 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23290 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23291 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23292 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23293 }
23294
23295 @Test
23296 public void testConstructor183() throws WrongSizeException {
23297 final Collection<StandardDeviationBodyKinematics> measurements =
23298 Collections.emptyList();
23299
23300 final Matrix ba = generateBa();
23301 final double biasX = ba.getElementAtIndex(0);
23302 final double biasY = ba.getElementAtIndex(1);
23303 final double biasZ = ba.getElementAtIndex(2);
23304
23305 final Matrix ma = generateMaCommonAxis();
23306 final double sx = ma.getElementAt(0, 0);
23307 final double sy = ma.getElementAt(1, 1);
23308 final double sz = ma.getElementAt(2, 2);
23309
23310 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23311 final double latitude = Math.toRadians(
23312 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23313 final double longitude = Math.toRadians(
23314 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23315 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23316 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23317 final NEDVelocity nedVelocity = new NEDVelocity();
23318 final ECEFPosition ecefPosition = new ECEFPosition();
23319 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23320 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23321 ecefPosition, ecefVelocity);
23322
23323 final KnownPositionAccelerometerCalibrator calibrator =
23324 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23325 biasX, biasY, biasZ, sx, sy, sz, this);
23326
23327
23328 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23329 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23330 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23331 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23332 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23333 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23334 final Acceleration bx2 = new Acceleration(0.0,
23335 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23336 calibrator.getInitialBiasXAsAcceleration(bx2);
23337 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23338 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23339 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23340 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23341 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23342 final Acceleration by2 = new Acceleration(0.0,
23343 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23344 calibrator.getInitialBiasYAsAcceleration(by2);
23345 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23346 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23347 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23348 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23349 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23350 final Acceleration bz2 = new Acceleration(0.0,
23351 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23352 calibrator.getInitialBiasZAsAcceleration(bz2);
23353 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23354 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23355 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23356 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23357 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23358 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23359 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23360 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23361 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23362 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23363 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23364 final double[] bias1 = calibrator.getInitialBias();
23365 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23366 final double[] bias2 = new double[3];
23367 calibrator.getInitialBias(bias2);
23368 assertArrayEquals(bias1, bias2, 0.0);
23369 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23370 assertEquals(b1, ba);
23371 final Matrix b2 = new Matrix(3, 1);
23372 calibrator.getInitialBiasAsMatrix(b2);
23373 assertEquals(b1, b2);
23374 final Matrix ma1 = calibrator.getInitialMa();
23375 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23376 final Matrix ma2 = new Matrix(3, 3);
23377 calibrator.getInitialMa(ma2);
23378 assertEquals(ma1, ma2);
23379 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23380 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23381 final NEDPosition nedPosition1 = new NEDPosition();
23382 assertTrue(calibrator.getNedPosition(nedPosition1));
23383 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23384 assertSame(calibrator.getMeasurements(), measurements);
23385 assertFalse(calibrator.isCommonAxisUsed());
23386 assertSame(calibrator.getListener(), this);
23387 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
23388 assertFalse(calibrator.isReady());
23389 assertFalse(calibrator.isRunning());
23390 assertNull(calibrator.getEstimatedBiases());
23391 assertFalse(calibrator.getEstimatedBiases(null));
23392 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23393 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23394 assertNull(calibrator.getEstimatedBiasFx());
23395 assertNull(calibrator.getEstimatedBiasFy());
23396 assertNull(calibrator.getEstimatedBiasFz());
23397 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23398 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23399 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23400 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23401 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23402 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23403 assertNull(calibrator.getEstimatedMa());
23404 assertNull(calibrator.getEstimatedSx());
23405 assertNull(calibrator.getEstimatedSy());
23406 assertNull(calibrator.getEstimatedSz());
23407 assertNull(calibrator.getEstimatedMxy());
23408 assertNull(calibrator.getEstimatedMxz());
23409 assertNull(calibrator.getEstimatedMyx());
23410 assertNull(calibrator.getEstimatedMyz());
23411 assertNull(calibrator.getEstimatedMzx());
23412 assertNull(calibrator.getEstimatedMzy());
23413 assertNull(calibrator.getEstimatedCovariance());
23414 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23415 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23416 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23417 assertNotNull(calibrator.getGroundTruthGravityNorm());
23418 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23419 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23420 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23421 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23422 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23423 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23424 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23425 }
23426
23427 @Test
23428 public void testConstructor184() throws WrongSizeException {
23429 final Matrix ba = generateBa();
23430 final double biasX = ba.getElementAtIndex(0);
23431 final double biasY = ba.getElementAtIndex(1);
23432 final double biasZ = ba.getElementAtIndex(2);
23433
23434 final Matrix ma = generateMaCommonAxis();
23435 final double sx = ma.getElementAt(0, 0);
23436 final double sy = ma.getElementAt(1, 1);
23437 final double sz = ma.getElementAt(2, 2);
23438
23439 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23440 final double latitude = Math.toRadians(
23441 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23442 final double longitude = Math.toRadians(
23443 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23444 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23445 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23446 final NEDVelocity nedVelocity = new NEDVelocity();
23447 final ECEFPosition ecefPosition = new ECEFPosition();
23448 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23449 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23450 ecefPosition, ecefVelocity);
23451
23452 final KnownPositionAccelerometerCalibrator calibrator =
23453 new KnownPositionAccelerometerCalibrator(nedPosition,
23454 true, biasX, biasY, biasZ, sx, sy, sz);
23455
23456
23457 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23458 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23459 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23460 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23461 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23462 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23463 final Acceleration bx2 = new Acceleration(0.0,
23464 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23465 calibrator.getInitialBiasXAsAcceleration(bx2);
23466 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23467 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23468 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23469 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23470 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23471 final Acceleration by2 = new Acceleration(0.0,
23472 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23473 calibrator.getInitialBiasYAsAcceleration(by2);
23474 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23475 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23476 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23477 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23478 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23479 final Acceleration bz2 = new Acceleration(0.0,
23480 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23481 calibrator.getInitialBiasZAsAcceleration(bz2);
23482 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23483 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23484 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23485 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23486 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23487 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23488 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23489 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23490 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23491 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23492 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23493 final double[] bias1 = calibrator.getInitialBias();
23494 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23495 final double[] bias2 = new double[3];
23496 calibrator.getInitialBias(bias2);
23497 assertArrayEquals(bias1, bias2, 0.0);
23498 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23499 assertEquals(b1, ba);
23500 final Matrix b2 = new Matrix(3, 1);
23501 calibrator.getInitialBiasAsMatrix(b2);
23502 assertEquals(b1, b2);
23503 final Matrix ma1 = calibrator.getInitialMa();
23504 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23505 final Matrix ma2 = new Matrix(3, 3);
23506 calibrator.getInitialMa(ma2);
23507 assertEquals(ma1, ma2);
23508 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23509 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23510 final NEDPosition nedPosition1 = new NEDPosition();
23511 assertTrue(calibrator.getNedPosition(nedPosition1));
23512 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23513 assertNull(calibrator.getMeasurements());
23514 assertTrue(calibrator.isCommonAxisUsed());
23515 assertNull(calibrator.getListener());
23516 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23517 assertFalse(calibrator.isReady());
23518 assertFalse(calibrator.isRunning());
23519 assertNull(calibrator.getEstimatedBiases());
23520 assertFalse(calibrator.getEstimatedBiases(null));
23521 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23522 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23523 assertNull(calibrator.getEstimatedBiasFx());
23524 assertNull(calibrator.getEstimatedBiasFy());
23525 assertNull(calibrator.getEstimatedBiasFz());
23526 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23527 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23528 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23529 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23530 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23531 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23532 assertNull(calibrator.getEstimatedMa());
23533 assertNull(calibrator.getEstimatedSx());
23534 assertNull(calibrator.getEstimatedSy());
23535 assertNull(calibrator.getEstimatedSz());
23536 assertNull(calibrator.getEstimatedMxy());
23537 assertNull(calibrator.getEstimatedMxz());
23538 assertNull(calibrator.getEstimatedMyx());
23539 assertNull(calibrator.getEstimatedMyz());
23540 assertNull(calibrator.getEstimatedMzx());
23541 assertNull(calibrator.getEstimatedMzy());
23542 assertNull(calibrator.getEstimatedCovariance());
23543 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23544 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23545 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23546 assertNotNull(calibrator.getGroundTruthGravityNorm());
23547 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23548 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23549 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23550 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23551 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23552 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23553 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23554 }
23555
23556 @Test
23557 public void testConstructor185() throws WrongSizeException {
23558 final Matrix ba = generateBa();
23559 final double biasX = ba.getElementAtIndex(0);
23560 final double biasY = ba.getElementAtIndex(1);
23561 final double biasZ = ba.getElementAtIndex(2);
23562
23563 final Matrix ma = generateMaCommonAxis();
23564 final double sx = ma.getElementAt(0, 0);
23565 final double sy = ma.getElementAt(1, 1);
23566 final double sz = ma.getElementAt(2, 2);
23567
23568 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23569 final double latitude = Math.toRadians(
23570 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23571 final double longitude = Math.toRadians(
23572 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23573 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23574 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23575 final NEDVelocity nedVelocity = new NEDVelocity();
23576 final ECEFPosition ecefPosition = new ECEFPosition();
23577 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23578 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23579 ecefPosition, ecefVelocity);
23580
23581 final KnownPositionAccelerometerCalibrator calibrator =
23582 new KnownPositionAccelerometerCalibrator(nedPosition,
23583 true, biasX, biasY, biasZ, sx, sy, sz,
23584 this);
23585
23586
23587 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23588 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23589 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23590 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23591 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23592 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23593 final Acceleration bx2 = new Acceleration(0.0,
23594 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23595 calibrator.getInitialBiasXAsAcceleration(bx2);
23596 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23597 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23598 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23599 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23600 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23601 final Acceleration by2 = new Acceleration(0.0,
23602 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23603 calibrator.getInitialBiasYAsAcceleration(by2);
23604 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23605 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23606 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23607 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23608 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23609 final Acceleration bz2 = new Acceleration(0.0,
23610 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23611 calibrator.getInitialBiasZAsAcceleration(bz2);
23612 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23613 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23614 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23615 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23616 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23617 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23618 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23619 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23620 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23621 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23622 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23623 final double[] bias1 = calibrator.getInitialBias();
23624 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23625 final double[] bias2 = new double[3];
23626 calibrator.getInitialBias(bias2);
23627 assertArrayEquals(bias1, bias2, 0.0);
23628 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23629 assertEquals(b1, ba);
23630 final Matrix b2 = new Matrix(3, 1);
23631 calibrator.getInitialBiasAsMatrix(b2);
23632 assertEquals(b1, b2);
23633 final Matrix ma1 = calibrator.getInitialMa();
23634 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23635 final Matrix ma2 = new Matrix(3, 3);
23636 calibrator.getInitialMa(ma2);
23637 assertEquals(ma1, ma2);
23638 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23639 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23640 final NEDPosition nedPosition1 = new NEDPosition();
23641 assertTrue(calibrator.getNedPosition(nedPosition1));
23642 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23643 assertNull(calibrator.getMeasurements());
23644 assertTrue(calibrator.isCommonAxisUsed());
23645 assertSame(calibrator.getListener(), this);
23646 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23647 assertFalse(calibrator.isReady());
23648 assertFalse(calibrator.isRunning());
23649 assertNull(calibrator.getEstimatedBiases());
23650 assertFalse(calibrator.getEstimatedBiases(null));
23651 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23652 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23653 assertNull(calibrator.getEstimatedBiasFx());
23654 assertNull(calibrator.getEstimatedBiasFy());
23655 assertNull(calibrator.getEstimatedBiasFz());
23656 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23657 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23658 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23659 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23660 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23661 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23662 assertNull(calibrator.getEstimatedMa());
23663 assertNull(calibrator.getEstimatedSx());
23664 assertNull(calibrator.getEstimatedSy());
23665 assertNull(calibrator.getEstimatedSz());
23666 assertNull(calibrator.getEstimatedMxy());
23667 assertNull(calibrator.getEstimatedMxz());
23668 assertNull(calibrator.getEstimatedMyx());
23669 assertNull(calibrator.getEstimatedMyz());
23670 assertNull(calibrator.getEstimatedMzx());
23671 assertNull(calibrator.getEstimatedMzy());
23672 assertNull(calibrator.getEstimatedCovariance());
23673 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23674 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23675 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23676 assertNotNull(calibrator.getGroundTruthGravityNorm());
23677 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23678 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23679 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23680 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23681 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23682 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23683 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23684 }
23685
23686 @Test
23687 public void testConstructor186() throws WrongSizeException {
23688 final Collection<StandardDeviationBodyKinematics> measurements =
23689 Collections.emptyList();
23690
23691 final Matrix ba = generateBa();
23692 final double biasX = ba.getElementAtIndex(0);
23693 final double biasY = ba.getElementAtIndex(1);
23694 final double biasZ = ba.getElementAtIndex(2);
23695
23696 final Matrix ma = generateMaCommonAxis();
23697 final double sx = ma.getElementAt(0, 0);
23698 final double sy = ma.getElementAt(1, 1);
23699 final double sz = ma.getElementAt(2, 2);
23700
23701 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23702 final double latitude = Math.toRadians(
23703 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23704 final double longitude = Math.toRadians(
23705 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23706 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23707 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23708 final NEDVelocity nedVelocity = new NEDVelocity();
23709 final ECEFPosition ecefPosition = new ECEFPosition();
23710 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23711 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23712 ecefPosition, ecefVelocity);
23713
23714 final KnownPositionAccelerometerCalibrator calibrator =
23715 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23716 true, biasX, biasY, biasZ, sx, sy, sz);
23717
23718
23719 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23720 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23721 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23722 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23723 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23724 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23725 final Acceleration bx2 = new Acceleration(0.0,
23726 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23727 calibrator.getInitialBiasXAsAcceleration(bx2);
23728 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23729 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23730 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23731 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23732 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23733 final Acceleration by2 = new Acceleration(0.0,
23734 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23735 calibrator.getInitialBiasYAsAcceleration(by2);
23736 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23737 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23738 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23739 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23740 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23741 final Acceleration bz2 = new Acceleration(0.0,
23742 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23743 calibrator.getInitialBiasZAsAcceleration(bz2);
23744 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23745 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23746 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23747 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23748 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23749 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23750 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23751 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23752 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23753 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23754 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23755 final double[] bias1 = calibrator.getInitialBias();
23756 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23757 final double[] bias2 = new double[3];
23758 calibrator.getInitialBias(bias2);
23759 assertArrayEquals(bias1, bias2, 0.0);
23760 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23761 assertEquals(b1, ba);
23762 final Matrix b2 = new Matrix(3, 1);
23763 calibrator.getInitialBiasAsMatrix(b2);
23764 assertEquals(b1, b2);
23765 final Matrix ma1 = calibrator.getInitialMa();
23766 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23767 final Matrix ma2 = new Matrix(3, 3);
23768 calibrator.getInitialMa(ma2);
23769 assertEquals(ma1, ma2);
23770 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23771 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23772 final NEDPosition nedPosition1 = new NEDPosition();
23773 assertTrue(calibrator.getNedPosition(nedPosition1));
23774 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23775 assertSame(calibrator.getMeasurements(), measurements);
23776 assertTrue(calibrator.isCommonAxisUsed());
23777 assertNull(calibrator.getListener());
23778 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23779 assertFalse(calibrator.isReady());
23780 assertFalse(calibrator.isRunning());
23781 assertNull(calibrator.getEstimatedBiases());
23782 assertFalse(calibrator.getEstimatedBiases(null));
23783 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23784 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23785 assertNull(calibrator.getEstimatedBiasFx());
23786 assertNull(calibrator.getEstimatedBiasFy());
23787 assertNull(calibrator.getEstimatedBiasFz());
23788 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23789 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23790 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23791 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23792 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23793 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23794 assertNull(calibrator.getEstimatedMa());
23795 assertNull(calibrator.getEstimatedSx());
23796 assertNull(calibrator.getEstimatedSy());
23797 assertNull(calibrator.getEstimatedSz());
23798 assertNull(calibrator.getEstimatedMxy());
23799 assertNull(calibrator.getEstimatedMxz());
23800 assertNull(calibrator.getEstimatedMyx());
23801 assertNull(calibrator.getEstimatedMyz());
23802 assertNull(calibrator.getEstimatedMzx());
23803 assertNull(calibrator.getEstimatedMzy());
23804 assertNull(calibrator.getEstimatedCovariance());
23805 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23806 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23807 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23808 assertNotNull(calibrator.getGroundTruthGravityNorm());
23809 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23810 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23811 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23812 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23813 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23814 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23815 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23816 }
23817
23818 @Test
23819 public void testConstructor187() throws WrongSizeException {
23820 final Collection<StandardDeviationBodyKinematics> measurements =
23821 Collections.emptyList();
23822
23823 final Matrix ba = generateBa();
23824 final double biasX = ba.getElementAtIndex(0);
23825 final double biasY = ba.getElementAtIndex(1);
23826 final double biasZ = ba.getElementAtIndex(2);
23827
23828 final Matrix ma = generateMaCommonAxis();
23829 final double sx = ma.getElementAt(0, 0);
23830 final double sy = ma.getElementAt(1, 1);
23831 final double sz = ma.getElementAt(2, 2);
23832
23833 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23834 final double latitude = Math.toRadians(
23835 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23836 final double longitude = Math.toRadians(
23837 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23838 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23839 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23840 final NEDVelocity nedVelocity = new NEDVelocity();
23841 final ECEFPosition ecefPosition = new ECEFPosition();
23842 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23843 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23844 ecefPosition, ecefVelocity);
23845
23846 final KnownPositionAccelerometerCalibrator calibrator =
23847 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
23848 true, biasX, biasY, biasZ, sx, sy, sz,
23849 this);
23850
23851
23852 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23853 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23854 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23855 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23856 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23857 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23858 final Acceleration bx2 = new Acceleration(0.0,
23859 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23860 calibrator.getInitialBiasXAsAcceleration(bx2);
23861 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23862 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23863 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
23864 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
23865 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23866 final Acceleration by2 = new Acceleration(0.0,
23867 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23868 calibrator.getInitialBiasYAsAcceleration(by2);
23869 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
23870 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23871 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
23872 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
23873 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23874 final Acceleration bz2 = new Acceleration(0.0,
23875 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23876 calibrator.getInitialBiasZAsAcceleration(bz2);
23877 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
23878 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23879 assertEquals(calibrator.getInitialSx(), sx, 0.0);
23880 assertEquals(calibrator.getInitialSy(), sy, 0.0);
23881 assertEquals(calibrator.getInitialSz(), sz, 0.0);
23882 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
23883 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
23884 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
23885 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
23886 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
23887 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
23888 final double[] bias1 = calibrator.getInitialBias();
23889 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
23890 final double[] bias2 = new double[3];
23891 calibrator.getInitialBias(bias2);
23892 assertArrayEquals(bias1, bias2, 0.0);
23893 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
23894 assertEquals(b1, ba);
23895 final Matrix b2 = new Matrix(3, 1);
23896 calibrator.getInitialBiasAsMatrix(b2);
23897 assertEquals(b1, b2);
23898 final Matrix ma1 = calibrator.getInitialMa();
23899 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
23900 final Matrix ma2 = new Matrix(3, 3);
23901 calibrator.getInitialMa(ma2);
23902 assertEquals(ma1, ma2);
23903 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
23904 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
23905 final NEDPosition nedPosition1 = new NEDPosition();
23906 assertTrue(calibrator.getNedPosition(nedPosition1));
23907 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
23908 assertSame(calibrator.getMeasurements(), measurements);
23909 assertTrue(calibrator.isCommonAxisUsed());
23910 assertSame(calibrator.getListener(), this);
23911 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
23912 assertFalse(calibrator.isReady());
23913 assertFalse(calibrator.isRunning());
23914 assertNull(calibrator.getEstimatedBiases());
23915 assertFalse(calibrator.getEstimatedBiases(null));
23916 assertNull(calibrator.getEstimatedBiasesAsMatrix());
23917 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
23918 assertNull(calibrator.getEstimatedBiasFx());
23919 assertNull(calibrator.getEstimatedBiasFy());
23920 assertNull(calibrator.getEstimatedBiasFz());
23921 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
23922 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
23923 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
23924 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
23925 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
23926 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
23927 assertNull(calibrator.getEstimatedMa());
23928 assertNull(calibrator.getEstimatedSx());
23929 assertNull(calibrator.getEstimatedSy());
23930 assertNull(calibrator.getEstimatedSz());
23931 assertNull(calibrator.getEstimatedMxy());
23932 assertNull(calibrator.getEstimatedMxz());
23933 assertNull(calibrator.getEstimatedMyx());
23934 assertNull(calibrator.getEstimatedMyz());
23935 assertNull(calibrator.getEstimatedMzx());
23936 assertNull(calibrator.getEstimatedMzy());
23937 assertNull(calibrator.getEstimatedCovariance());
23938 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
23939 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
23940 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
23941 assertNotNull(calibrator.getGroundTruthGravityNorm());
23942 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
23943 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
23944 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
23945 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
23946 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
23947 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
23948 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
23949 }
23950
23951 @Test
23952 public void testConstructor188() throws WrongSizeException {
23953 final Matrix ba = generateBa();
23954 final double biasX = ba.getElementAtIndex(0);
23955 final double biasY = ba.getElementAtIndex(1);
23956 final double biasZ = ba.getElementAtIndex(2);
23957
23958 final Acceleration bx = new Acceleration(biasX,
23959 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23960 final Acceleration by = new Acceleration(biasY,
23961 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23962 final Acceleration bz = new Acceleration(biasZ,
23963 AccelerationUnit.METERS_PER_SQUARED_SECOND);
23964
23965 final Matrix ma = generateMaCommonAxis();
23966 final double sx = ma.getElementAt(0, 0);
23967 final double sy = ma.getElementAt(1, 1);
23968 final double sz = ma.getElementAt(2, 2);
23969
23970 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
23971 final double latitude = Math.toRadians(
23972 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
23973 final double longitude = Math.toRadians(
23974 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
23975 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
23976 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
23977 final NEDVelocity nedVelocity = new NEDVelocity();
23978 final ECEFPosition ecefPosition = new ECEFPosition();
23979 final ECEFVelocity ecefVelocity = new ECEFVelocity();
23980 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
23981 ecefPosition, ecefVelocity);
23982
23983 final KnownPositionAccelerometerCalibrator calibrator =
23984 new KnownPositionAccelerometerCalibrator(nedPosition,
23985 bx, by, bz, sx, sy, sz);
23986
23987
23988 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
23989 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
23990 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
23991 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
23992 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
23993 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23994 final Acceleration bx2 = new Acceleration(0.0,
23995 AccelerationUnit.FEET_PER_SQUARED_SECOND);
23996 calibrator.getInitialBiasXAsAcceleration(bx2);
23997 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
23998 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
23999 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24000 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24001 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24002 final Acceleration by2 = new Acceleration(0.0,
24003 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24004 calibrator.getInitialBiasYAsAcceleration(by2);
24005 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24006 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24007 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24008 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24009 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24010 final Acceleration bz2 = new Acceleration(0.0,
24011 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24012 calibrator.getInitialBiasZAsAcceleration(bz2);
24013 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24014 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24015 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24016 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24017 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24018 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24019 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24020 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24021 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24022 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24023 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24024 final double[] bias1 = calibrator.getInitialBias();
24025 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24026 final double[] bias2 = new double[3];
24027 calibrator.getInitialBias(bias2);
24028 assertArrayEquals(bias1, bias2, 0.0);
24029 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24030 assertEquals(b1, ba);
24031 final Matrix b2 = new Matrix(3, 1);
24032 calibrator.getInitialBiasAsMatrix(b2);
24033 assertEquals(b1, b2);
24034 final Matrix ma1 = calibrator.getInitialMa();
24035 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24036 final Matrix ma2 = new Matrix(3, 3);
24037 calibrator.getInitialMa(ma2);
24038 assertEquals(ma1, ma2);
24039 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24040 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24041 final NEDPosition nedPosition1 = new NEDPosition();
24042 assertTrue(calibrator.getNedPosition(nedPosition1));
24043 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24044 assertNull(calibrator.getMeasurements());
24045 assertFalse(calibrator.isCommonAxisUsed());
24046 assertNull(calibrator.getListener());
24047 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24048 assertFalse(calibrator.isReady());
24049 assertFalse(calibrator.isRunning());
24050 assertNull(calibrator.getEstimatedBiases());
24051 assertFalse(calibrator.getEstimatedBiases(null));
24052 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24053 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24054 assertNull(calibrator.getEstimatedBiasFx());
24055 assertNull(calibrator.getEstimatedBiasFy());
24056 assertNull(calibrator.getEstimatedBiasFz());
24057 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24058 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24059 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24060 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24061 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24062 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24063 assertNull(calibrator.getEstimatedMa());
24064 assertNull(calibrator.getEstimatedSx());
24065 assertNull(calibrator.getEstimatedSy());
24066 assertNull(calibrator.getEstimatedSz());
24067 assertNull(calibrator.getEstimatedMxy());
24068 assertNull(calibrator.getEstimatedMxz());
24069 assertNull(calibrator.getEstimatedMyx());
24070 assertNull(calibrator.getEstimatedMyz());
24071 assertNull(calibrator.getEstimatedMzx());
24072 assertNull(calibrator.getEstimatedMzy());
24073 assertNull(calibrator.getEstimatedCovariance());
24074 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24075 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24076 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24077 assertNotNull(calibrator.getGroundTruthGravityNorm());
24078 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24079 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24080 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24081 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24082 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24083 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24084 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24085 }
24086
24087 @Test
24088 public void testConstructor189() throws WrongSizeException {
24089 final Matrix ba = generateBa();
24090 final double biasX = ba.getElementAtIndex(0);
24091 final double biasY = ba.getElementAtIndex(1);
24092 final double biasZ = ba.getElementAtIndex(2);
24093
24094 final Acceleration bx = new Acceleration(biasX,
24095 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24096 final Acceleration by = new Acceleration(biasY,
24097 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24098 final Acceleration bz = new Acceleration(biasZ,
24099 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24100
24101 final Matrix ma = generateMaCommonAxis();
24102 final double sx = ma.getElementAt(0, 0);
24103 final double sy = ma.getElementAt(1, 1);
24104 final double sz = ma.getElementAt(2, 2);
24105
24106 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24107 final double latitude = Math.toRadians(
24108 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24109 final double longitude = Math.toRadians(
24110 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24111 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24112 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24113 final NEDVelocity nedVelocity = new NEDVelocity();
24114 final ECEFPosition ecefPosition = new ECEFPosition();
24115 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24116 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24117 ecefPosition, ecefVelocity);
24118
24119 final KnownPositionAccelerometerCalibrator calibrator =
24120 new KnownPositionAccelerometerCalibrator(nedPosition,
24121 bx, by, bz, sx, sy, sz, this);
24122
24123
24124 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24125 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24126 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24127 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24128 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24129 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24130 final Acceleration bx2 = new Acceleration(0.0,
24131 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24132 calibrator.getInitialBiasXAsAcceleration(bx2);
24133 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24134 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24135 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24136 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24137 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24138 final Acceleration by2 = new Acceleration(0.0,
24139 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24140 calibrator.getInitialBiasYAsAcceleration(by2);
24141 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24142 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24143 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24144 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24145 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24146 final Acceleration bz2 = new Acceleration(0.0,
24147 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24148 calibrator.getInitialBiasZAsAcceleration(bz2);
24149 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24150 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24151 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24152 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24153 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24154 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24155 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24156 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24157 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24158 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24159 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24160 final double[] bias1 = calibrator.getInitialBias();
24161 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24162 final double[] bias2 = new double[3];
24163 calibrator.getInitialBias(bias2);
24164 assertArrayEquals(bias1, bias2, 0.0);
24165 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24166 assertEquals(b1, ba);
24167 final Matrix b2 = new Matrix(3, 1);
24168 calibrator.getInitialBiasAsMatrix(b2);
24169 assertEquals(b1, b2);
24170 final Matrix ma1 = calibrator.getInitialMa();
24171 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24172 final Matrix ma2 = new Matrix(3, 3);
24173 calibrator.getInitialMa(ma2);
24174 assertEquals(ma1, ma2);
24175 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24176 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24177 final NEDPosition nedPosition1 = new NEDPosition();
24178 assertTrue(calibrator.getNedPosition(nedPosition1));
24179 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24180 assertNull(calibrator.getMeasurements());
24181 assertFalse(calibrator.isCommonAxisUsed());
24182 assertSame(calibrator.getListener(), this);
24183 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24184 assertFalse(calibrator.isReady());
24185 assertFalse(calibrator.isRunning());
24186 assertNull(calibrator.getEstimatedBiases());
24187 assertFalse(calibrator.getEstimatedBiases(null));
24188 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24189 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24190 assertNull(calibrator.getEstimatedBiasFx());
24191 assertNull(calibrator.getEstimatedBiasFy());
24192 assertNull(calibrator.getEstimatedBiasFz());
24193 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24194 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24195 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24196 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24197 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24198 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24199 assertNull(calibrator.getEstimatedMa());
24200 assertNull(calibrator.getEstimatedSx());
24201 assertNull(calibrator.getEstimatedSy());
24202 assertNull(calibrator.getEstimatedSz());
24203 assertNull(calibrator.getEstimatedMxy());
24204 assertNull(calibrator.getEstimatedMxz());
24205 assertNull(calibrator.getEstimatedMyx());
24206 assertNull(calibrator.getEstimatedMyz());
24207 assertNull(calibrator.getEstimatedMzx());
24208 assertNull(calibrator.getEstimatedMzy());
24209 assertNull(calibrator.getEstimatedCovariance());
24210 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24211 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24212 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24213 assertNotNull(calibrator.getGroundTruthGravityNorm());
24214 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24215 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24216 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24217 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24218 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24219 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24220 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24221 }
24222
24223 @Test
24224 public void testConstructor190() throws WrongSizeException {
24225 final Collection<StandardDeviationBodyKinematics> measurements =
24226 Collections.emptyList();
24227
24228 final Matrix ba = generateBa();
24229 final double biasX = ba.getElementAtIndex(0);
24230 final double biasY = ba.getElementAtIndex(1);
24231 final double biasZ = ba.getElementAtIndex(2);
24232
24233 final Acceleration bx = new Acceleration(biasX,
24234 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24235 final Acceleration by = new Acceleration(biasY,
24236 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24237 final Acceleration bz = new Acceleration(biasZ,
24238 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24239
24240 final Matrix ma = generateMaCommonAxis();
24241 final double sx = ma.getElementAt(0, 0);
24242 final double sy = ma.getElementAt(1, 1);
24243 final double sz = ma.getElementAt(2, 2);
24244
24245 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24246 final double latitude = Math.toRadians(
24247 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24248 final double longitude = Math.toRadians(
24249 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24250 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24251 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24252 final NEDVelocity nedVelocity = new NEDVelocity();
24253 final ECEFPosition ecefPosition = new ECEFPosition();
24254 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24255 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24256 ecefPosition, ecefVelocity);
24257
24258 final KnownPositionAccelerometerCalibrator calibrator =
24259 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24260 bx, by, bz, sx, sy, sz);
24261
24262
24263 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24264 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24265 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24266 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24267 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24268 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24269 final Acceleration bx2 = new Acceleration(0.0,
24270 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24271 calibrator.getInitialBiasXAsAcceleration(bx2);
24272 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24273 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24274 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24275 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24276 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24277 final Acceleration by2 = new Acceleration(0.0,
24278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24279 calibrator.getInitialBiasYAsAcceleration(by2);
24280 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24281 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24282 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24283 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24284 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24285 final Acceleration bz2 = new Acceleration(0.0,
24286 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24287 calibrator.getInitialBiasZAsAcceleration(bz2);
24288 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24289 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24290 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24291 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24292 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24293 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24294 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24295 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24296 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24297 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24298 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24299 final double[] bias1 = calibrator.getInitialBias();
24300 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24301 final double[] bias2 = new double[3];
24302 calibrator.getInitialBias(bias2);
24303 assertArrayEquals(bias1, bias2, 0.0);
24304 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24305 assertEquals(b1, ba);
24306 final Matrix b2 = new Matrix(3, 1);
24307 calibrator.getInitialBiasAsMatrix(b2);
24308 assertEquals(b1, b2);
24309 final Matrix ma1 = calibrator.getInitialMa();
24310 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24311 final Matrix ma2 = new Matrix(3, 3);
24312 calibrator.getInitialMa(ma2);
24313 assertEquals(ma1, ma2);
24314 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24315 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24316 final NEDPosition nedPosition1 = new NEDPosition();
24317 assertTrue(calibrator.getNedPosition(nedPosition1));
24318 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24319 assertSame(calibrator.getMeasurements(), measurements);
24320 assertFalse(calibrator.isCommonAxisUsed());
24321 assertNull(calibrator.getListener());
24322 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24323 assertFalse(calibrator.isReady());
24324 assertFalse(calibrator.isRunning());
24325 assertNull(calibrator.getEstimatedBiases());
24326 assertFalse(calibrator.getEstimatedBiases(null));
24327 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24328 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24329 assertNull(calibrator.getEstimatedBiasFx());
24330 assertNull(calibrator.getEstimatedBiasFy());
24331 assertNull(calibrator.getEstimatedBiasFz());
24332 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24333 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24334 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24335 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24336 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24337 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24338 assertNull(calibrator.getEstimatedMa());
24339 assertNull(calibrator.getEstimatedSx());
24340 assertNull(calibrator.getEstimatedSy());
24341 assertNull(calibrator.getEstimatedSz());
24342 assertNull(calibrator.getEstimatedMxy());
24343 assertNull(calibrator.getEstimatedMxz());
24344 assertNull(calibrator.getEstimatedMyx());
24345 assertNull(calibrator.getEstimatedMyz());
24346 assertNull(calibrator.getEstimatedMzx());
24347 assertNull(calibrator.getEstimatedMzy());
24348 assertNull(calibrator.getEstimatedCovariance());
24349 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24350 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24351 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24352 assertNotNull(calibrator.getGroundTruthGravityNorm());
24353 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24354 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24355 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24356 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24357 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24358 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24359 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24360 }
24361
24362 @Test
24363 public void testConstructor191() throws WrongSizeException {
24364 final Collection<StandardDeviationBodyKinematics> measurements =
24365 Collections.emptyList();
24366
24367 final Matrix ba = generateBa();
24368 final double biasX = ba.getElementAtIndex(0);
24369 final double biasY = ba.getElementAtIndex(1);
24370 final double biasZ = ba.getElementAtIndex(2);
24371
24372 final Acceleration bx = new Acceleration(biasX,
24373 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24374 final Acceleration by = new Acceleration(biasY,
24375 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24376 final Acceleration bz = new Acceleration(biasZ,
24377 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24378
24379 final Matrix ma = generateMaCommonAxis();
24380 final double sx = ma.getElementAt(0, 0);
24381 final double sy = ma.getElementAt(1, 1);
24382 final double sz = ma.getElementAt(2, 2);
24383
24384 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24385 final double latitude = Math.toRadians(
24386 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24387 final double longitude = Math.toRadians(
24388 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24389 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24390 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24391 final NEDVelocity nedVelocity = new NEDVelocity();
24392 final ECEFPosition ecefPosition = new ECEFPosition();
24393 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24394 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24395 ecefPosition, ecefVelocity);
24396
24397 final KnownPositionAccelerometerCalibrator calibrator =
24398 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24399 bx, by, bz, sx, sy, sz, this);
24400
24401
24402 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24403 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24404 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24405 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24406 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24407 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24408 final Acceleration bx2 = new Acceleration(0.0,
24409 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24410 calibrator.getInitialBiasXAsAcceleration(bx2);
24411 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24412 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24413 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24414 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24415 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24416 final Acceleration by2 = new Acceleration(0.0,
24417 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24418 calibrator.getInitialBiasYAsAcceleration(by2);
24419 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24420 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24421 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24422 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24423 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24424 final Acceleration bz2 = new Acceleration(0.0,
24425 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24426 calibrator.getInitialBiasZAsAcceleration(bz2);
24427 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24428 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24429 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24430 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24431 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24432 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24433 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24434 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24435 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24436 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24437 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24438 final double[] bias1 = calibrator.getInitialBias();
24439 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24440 final double[] bias2 = new double[3];
24441 calibrator.getInitialBias(bias2);
24442 assertArrayEquals(bias1, bias2, 0.0);
24443 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24444 assertEquals(b1, ba);
24445 final Matrix b2 = new Matrix(3, 1);
24446 calibrator.getInitialBiasAsMatrix(b2);
24447 assertEquals(b1, b2);
24448 final Matrix ma1 = calibrator.getInitialMa();
24449 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24450 final Matrix ma2 = new Matrix(3, 3);
24451 calibrator.getInitialMa(ma2);
24452 assertEquals(ma1, ma2);
24453 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24454 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24455 final NEDPosition nedPosition1 = new NEDPosition();
24456 assertTrue(calibrator.getNedPosition(nedPosition1));
24457 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24458 assertSame(calibrator.getMeasurements(), measurements);
24459 assertFalse(calibrator.isCommonAxisUsed());
24460 assertSame(calibrator.getListener(), this);
24461 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
24462 assertFalse(calibrator.isReady());
24463 assertFalse(calibrator.isRunning());
24464 assertNull(calibrator.getEstimatedBiases());
24465 assertFalse(calibrator.getEstimatedBiases(null));
24466 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24467 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24468 assertNull(calibrator.getEstimatedBiasFx());
24469 assertNull(calibrator.getEstimatedBiasFy());
24470 assertNull(calibrator.getEstimatedBiasFz());
24471 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24472 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24473 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24474 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24475 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24476 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24477 assertNull(calibrator.getEstimatedMa());
24478 assertNull(calibrator.getEstimatedSx());
24479 assertNull(calibrator.getEstimatedSy());
24480 assertNull(calibrator.getEstimatedSz());
24481 assertNull(calibrator.getEstimatedMxy());
24482 assertNull(calibrator.getEstimatedMxz());
24483 assertNull(calibrator.getEstimatedMyx());
24484 assertNull(calibrator.getEstimatedMyz());
24485 assertNull(calibrator.getEstimatedMzx());
24486 assertNull(calibrator.getEstimatedMzy());
24487 assertNull(calibrator.getEstimatedCovariance());
24488 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24489 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24490 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24491 assertNotNull(calibrator.getGroundTruthGravityNorm());
24492 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24493 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24494 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24495 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24496 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24497 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24498 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24499 }
24500
24501 @Test
24502 public void testConstructor192() throws WrongSizeException {
24503 final Matrix ba = generateBa();
24504 final double biasX = ba.getElementAtIndex(0);
24505 final double biasY = ba.getElementAtIndex(1);
24506 final double biasZ = ba.getElementAtIndex(2);
24507
24508 final Acceleration bx = new Acceleration(biasX,
24509 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24510 final Acceleration by = new Acceleration(biasY,
24511 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24512 final Acceleration bz = new Acceleration(biasZ,
24513 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24514
24515 final Matrix ma = generateMaCommonAxis();
24516 final double sx = ma.getElementAt(0, 0);
24517 final double sy = ma.getElementAt(1, 1);
24518 final double sz = ma.getElementAt(2, 2);
24519
24520 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24521 final double latitude = Math.toRadians(
24522 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24523 final double longitude = Math.toRadians(
24524 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24525 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24526 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24527 final NEDVelocity nedVelocity = new NEDVelocity();
24528 final ECEFPosition ecefPosition = new ECEFPosition();
24529 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24530 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24531 ecefPosition, ecefVelocity);
24532
24533 final KnownPositionAccelerometerCalibrator calibrator =
24534 new KnownPositionAccelerometerCalibrator(nedPosition,
24535 true, bx, by, bz, sx, sy, sz);
24536
24537
24538 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24539 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24540 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24541 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24542 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24543 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24544 final Acceleration bx2 = new Acceleration(0.0,
24545 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24546 calibrator.getInitialBiasXAsAcceleration(bx2);
24547 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24548 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24549 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24550 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24551 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24552 final Acceleration by2 = new Acceleration(0.0,
24553 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24554 calibrator.getInitialBiasYAsAcceleration(by2);
24555 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24556 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24557 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24558 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24559 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24560 final Acceleration bz2 = new Acceleration(0.0,
24561 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24562 calibrator.getInitialBiasZAsAcceleration(bz2);
24563 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24564 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24565 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24566 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24567 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24568 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24569 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24570 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24571 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24572 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24573 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24574 final double[] bias1 = calibrator.getInitialBias();
24575 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24576 final double[] bias2 = new double[3];
24577 calibrator.getInitialBias(bias2);
24578 assertArrayEquals(bias1, bias2, 0.0);
24579 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24580 assertEquals(b1, ba);
24581 final Matrix b2 = new Matrix(3, 1);
24582 calibrator.getInitialBiasAsMatrix(b2);
24583 assertEquals(b1, b2);
24584 final Matrix ma1 = calibrator.getInitialMa();
24585 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24586 final Matrix ma2 = new Matrix(3, 3);
24587 calibrator.getInitialMa(ma2);
24588 assertEquals(ma1, ma2);
24589 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24590 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24591 final NEDPosition nedPosition1 = new NEDPosition();
24592 assertTrue(calibrator.getNedPosition(nedPosition1));
24593 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24594 assertNull(calibrator.getMeasurements());
24595 assertTrue(calibrator.isCommonAxisUsed());
24596 assertNull(calibrator.getListener());
24597 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24598 assertFalse(calibrator.isReady());
24599 assertFalse(calibrator.isRunning());
24600 assertNull(calibrator.getEstimatedBiases());
24601 assertFalse(calibrator.getEstimatedBiases(null));
24602 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24603 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24604 assertNull(calibrator.getEstimatedBiasFx());
24605 assertNull(calibrator.getEstimatedBiasFy());
24606 assertNull(calibrator.getEstimatedBiasFz());
24607 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24608 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24609 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24610 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24611 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24612 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24613 assertNull(calibrator.getEstimatedMa());
24614 assertNull(calibrator.getEstimatedSx());
24615 assertNull(calibrator.getEstimatedSy());
24616 assertNull(calibrator.getEstimatedSz());
24617 assertNull(calibrator.getEstimatedMxy());
24618 assertNull(calibrator.getEstimatedMxz());
24619 assertNull(calibrator.getEstimatedMyx());
24620 assertNull(calibrator.getEstimatedMyz());
24621 assertNull(calibrator.getEstimatedMzx());
24622 assertNull(calibrator.getEstimatedMzy());
24623 assertNull(calibrator.getEstimatedCovariance());
24624 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24625 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24626 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24627 assertNotNull(calibrator.getGroundTruthGravityNorm());
24628 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24629 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24630 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24631 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24632 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24633 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24634 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24635 }
24636
24637 @Test
24638 public void testConstructor193() throws WrongSizeException {
24639 final Matrix ba = generateBa();
24640 final double biasX = ba.getElementAtIndex(0);
24641 final double biasY = ba.getElementAtIndex(1);
24642 final double biasZ = ba.getElementAtIndex(2);
24643
24644 final Acceleration bx = new Acceleration(biasX,
24645 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24646 final Acceleration by = new Acceleration(biasY,
24647 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24648 final Acceleration bz = new Acceleration(biasZ,
24649 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24650
24651 final Matrix ma = generateMaCommonAxis();
24652 final double sx = ma.getElementAt(0, 0);
24653 final double sy = ma.getElementAt(1, 1);
24654 final double sz = ma.getElementAt(2, 2);
24655
24656 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24657 final double latitude = Math.toRadians(
24658 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24659 final double longitude = Math.toRadians(
24660 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24661 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24662 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24663 final NEDVelocity nedVelocity = new NEDVelocity();
24664 final ECEFPosition ecefPosition = new ECEFPosition();
24665 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24666 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24667 ecefPosition, ecefVelocity);
24668
24669 final KnownPositionAccelerometerCalibrator calibrator =
24670 new KnownPositionAccelerometerCalibrator(nedPosition,
24671 true, bx, by, bz, sx, sy, sz, this);
24672
24673
24674 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24675 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24676 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24677 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24678 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24679 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24680 final Acceleration bx2 = new Acceleration(0.0,
24681 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24682 calibrator.getInitialBiasXAsAcceleration(bx2);
24683 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24684 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24685 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24686 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24687 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24688 final Acceleration by2 = new Acceleration(0.0,
24689 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24690 calibrator.getInitialBiasYAsAcceleration(by2);
24691 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24692 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24693 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24694 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24695 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24696 final Acceleration bz2 = new Acceleration(0.0,
24697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24698 calibrator.getInitialBiasZAsAcceleration(bz2);
24699 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24700 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24701 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24702 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24703 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24704 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24705 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24706 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24707 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24708 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24709 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24710 final double[] bias1 = calibrator.getInitialBias();
24711 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24712 final double[] bias2 = new double[3];
24713 calibrator.getInitialBias(bias2);
24714 assertArrayEquals(bias1, bias2, 0.0);
24715 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24716 assertEquals(b1, ba);
24717 final Matrix b2 = new Matrix(3, 1);
24718 calibrator.getInitialBiasAsMatrix(b2);
24719 assertEquals(b1, b2);
24720 final Matrix ma1 = calibrator.getInitialMa();
24721 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24722 final Matrix ma2 = new Matrix(3, 3);
24723 calibrator.getInitialMa(ma2);
24724 assertEquals(ma1, ma2);
24725 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24726 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24727 final NEDPosition nedPosition1 = new NEDPosition();
24728 assertTrue(calibrator.getNedPosition(nedPosition1));
24729 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24730 assertNull(calibrator.getMeasurements());
24731 assertTrue(calibrator.isCommonAxisUsed());
24732 assertSame(calibrator.getListener(), this);
24733 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24734 assertFalse(calibrator.isReady());
24735 assertFalse(calibrator.isRunning());
24736 assertNull(calibrator.getEstimatedBiases());
24737 assertFalse(calibrator.getEstimatedBiases(null));
24738 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24739 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24740 assertNull(calibrator.getEstimatedBiasFx());
24741 assertNull(calibrator.getEstimatedBiasFy());
24742 assertNull(calibrator.getEstimatedBiasFz());
24743 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24744 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24745 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24746 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24747 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24748 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24749 assertNull(calibrator.getEstimatedMa());
24750 assertNull(calibrator.getEstimatedSx());
24751 assertNull(calibrator.getEstimatedSy());
24752 assertNull(calibrator.getEstimatedSz());
24753 assertNull(calibrator.getEstimatedMxy());
24754 assertNull(calibrator.getEstimatedMxz());
24755 assertNull(calibrator.getEstimatedMyx());
24756 assertNull(calibrator.getEstimatedMyz());
24757 assertNull(calibrator.getEstimatedMzx());
24758 assertNull(calibrator.getEstimatedMzy());
24759 assertNull(calibrator.getEstimatedCovariance());
24760 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24761 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24762 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24763 assertNotNull(calibrator.getGroundTruthGravityNorm());
24764 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24765 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24766 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24767 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24768 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24769 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24770 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24771 }
24772
24773 @Test
24774 public void testConstructor194() throws WrongSizeException {
24775 final Collection<StandardDeviationBodyKinematics> measurements =
24776 Collections.emptyList();
24777
24778 final Matrix ba = generateBa();
24779 final double biasX = ba.getElementAtIndex(0);
24780 final double biasY = ba.getElementAtIndex(1);
24781 final double biasZ = ba.getElementAtIndex(2);
24782
24783 final Acceleration bx = new Acceleration(biasX,
24784 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24785 final Acceleration by = new Acceleration(biasY,
24786 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24787 final Acceleration bz = new Acceleration(biasZ,
24788 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24789
24790 final Matrix ma = generateMaCommonAxis();
24791 final double sx = ma.getElementAt(0, 0);
24792 final double sy = ma.getElementAt(1, 1);
24793 final double sz = ma.getElementAt(2, 2);
24794
24795 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24796 final double latitude = Math.toRadians(
24797 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24798 final double longitude = Math.toRadians(
24799 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24800 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24801 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24802 final NEDVelocity nedVelocity = new NEDVelocity();
24803 final ECEFPosition ecefPosition = new ECEFPosition();
24804 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24805 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24806 ecefPosition, ecefVelocity);
24807
24808 final KnownPositionAccelerometerCalibrator calibrator =
24809 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24810 true, bx, by, bz, sx, sy, sz);
24811
24812
24813 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24814 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24815 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24816 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24817 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24818 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24819 final Acceleration bx2 = new Acceleration(0.0,
24820 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24821 calibrator.getInitialBiasXAsAcceleration(bx2);
24822 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24823 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24824 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24825 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24826 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24827 final Acceleration by2 = new Acceleration(0.0,
24828 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24829 calibrator.getInitialBiasYAsAcceleration(by2);
24830 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24831 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24832 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24833 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24834 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24835 final Acceleration bz2 = new Acceleration(0.0,
24836 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24837 calibrator.getInitialBiasZAsAcceleration(bz2);
24838 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24839 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24840 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24841 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24842 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24843 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24844 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24845 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24846 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24847 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24848 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24849 final double[] bias1 = calibrator.getInitialBias();
24850 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24851 final double[] bias2 = new double[3];
24852 calibrator.getInitialBias(bias2);
24853 assertArrayEquals(bias1, bias2, 0.0);
24854 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24855 assertEquals(b1, ba);
24856 final Matrix b2 = new Matrix(3, 1);
24857 calibrator.getInitialBiasAsMatrix(b2);
24858 assertEquals(b1, b2);
24859 final Matrix ma1 = calibrator.getInitialMa();
24860 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
24861 final Matrix ma2 = new Matrix(3, 3);
24862 calibrator.getInitialMa(ma2);
24863 assertEquals(ma1, ma2);
24864 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
24865 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
24866 final NEDPosition nedPosition1 = new NEDPosition();
24867 assertTrue(calibrator.getNedPosition(nedPosition1));
24868 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
24869 assertSame(calibrator.getMeasurements(), measurements);
24870 assertTrue(calibrator.isCommonAxisUsed());
24871 assertNull(calibrator.getListener());
24872 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
24873 assertFalse(calibrator.isReady());
24874 assertFalse(calibrator.isRunning());
24875 assertNull(calibrator.getEstimatedBiases());
24876 assertFalse(calibrator.getEstimatedBiases(null));
24877 assertNull(calibrator.getEstimatedBiasesAsMatrix());
24878 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
24879 assertNull(calibrator.getEstimatedBiasFx());
24880 assertNull(calibrator.getEstimatedBiasFy());
24881 assertNull(calibrator.getEstimatedBiasFz());
24882 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
24883 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
24884 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
24885 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
24886 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
24887 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
24888 assertNull(calibrator.getEstimatedMa());
24889 assertNull(calibrator.getEstimatedSx());
24890 assertNull(calibrator.getEstimatedSy());
24891 assertNull(calibrator.getEstimatedSz());
24892 assertNull(calibrator.getEstimatedMxy());
24893 assertNull(calibrator.getEstimatedMxz());
24894 assertNull(calibrator.getEstimatedMyx());
24895 assertNull(calibrator.getEstimatedMyz());
24896 assertNull(calibrator.getEstimatedMzx());
24897 assertNull(calibrator.getEstimatedMzy());
24898 assertNull(calibrator.getEstimatedCovariance());
24899 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
24900 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
24901 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
24902 assertNotNull(calibrator.getGroundTruthGravityNorm());
24903 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
24904 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
24905 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
24906 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
24907 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
24908 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
24909 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
24910 }
24911
24912 @Test
24913 public void testConstructor195() throws WrongSizeException {
24914 final Collection<StandardDeviationBodyKinematics> measurements =
24915 Collections.emptyList();
24916
24917 final Matrix ba = generateBa();
24918 final double biasX = ba.getElementAtIndex(0);
24919 final double biasY = ba.getElementAtIndex(1);
24920 final double biasZ = ba.getElementAtIndex(2);
24921
24922 final Acceleration bx = new Acceleration(biasX,
24923 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24924 final Acceleration by = new Acceleration(biasY,
24925 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24926 final Acceleration bz = new Acceleration(biasZ,
24927 AccelerationUnit.METERS_PER_SQUARED_SECOND);
24928
24929 final Matrix ma = generateMaCommonAxis();
24930 final double sx = ma.getElementAt(0, 0);
24931 final double sy = ma.getElementAt(1, 1);
24932 final double sz = ma.getElementAt(2, 2);
24933
24934 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
24935 final double latitude = Math.toRadians(
24936 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
24937 final double longitude = Math.toRadians(
24938 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
24939 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
24940 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
24941 final NEDVelocity nedVelocity = new NEDVelocity();
24942 final ECEFPosition ecefPosition = new ECEFPosition();
24943 final ECEFVelocity ecefVelocity = new ECEFVelocity();
24944 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
24945 ecefPosition, ecefVelocity);
24946
24947 final KnownPositionAccelerometerCalibrator calibrator =
24948 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
24949 true, bx, by, bz, sx, sy, sz, this);
24950
24951
24952 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
24953 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
24954 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
24955 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
24956 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
24957 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24958 final Acceleration bx2 = new Acceleration(0.0,
24959 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24960 calibrator.getInitialBiasXAsAcceleration(bx2);
24961 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
24962 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24963 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
24964 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
24965 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24966 final Acceleration by2 = new Acceleration(0.0,
24967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24968 calibrator.getInitialBiasYAsAcceleration(by2);
24969 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
24970 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24971 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
24972 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
24973 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24974 final Acceleration bz2 = new Acceleration(0.0,
24975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
24976 calibrator.getInitialBiasZAsAcceleration(bz2);
24977 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
24978 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
24979 assertEquals(calibrator.getInitialSx(), sx, 0.0);
24980 assertEquals(calibrator.getInitialSy(), sy, 0.0);
24981 assertEquals(calibrator.getInitialSz(), sz, 0.0);
24982 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
24983 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
24984 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
24985 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
24986 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
24987 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
24988 final double[] bias1 = calibrator.getInitialBias();
24989 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
24990 final double[] bias2 = new double[3];
24991 calibrator.getInitialBias(bias2);
24992 assertArrayEquals(bias1, bias2, 0.0);
24993 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
24994 assertEquals(b1, ba);
24995 final Matrix b2 = new Matrix(3, 1);
24996 calibrator.getInitialBiasAsMatrix(b2);
24997 assertEquals(b1, b2);
24998 final Matrix ma1 = calibrator.getInitialMa();
24999 assertEquals(ma1, Matrix.diagonal(new double[]{sx, sy, sz}));
25000 final Matrix ma2 = new Matrix(3, 3);
25001 calibrator.getInitialMa(ma2);
25002 assertEquals(ma1, ma2);
25003 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25004 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25005 final NEDPosition nedPosition1 = new NEDPosition();
25006 assertTrue(calibrator.getNedPosition(nedPosition1));
25007 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25008 assertSame(calibrator.getMeasurements(), measurements);
25009 assertTrue(calibrator.isCommonAxisUsed());
25010 assertSame(calibrator.getListener(), this);
25011 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25012 assertFalse(calibrator.isReady());
25013 assertFalse(calibrator.isRunning());
25014 assertNull(calibrator.getEstimatedBiases());
25015 assertFalse(calibrator.getEstimatedBiases(null));
25016 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25017 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25018 assertNull(calibrator.getEstimatedBiasFx());
25019 assertNull(calibrator.getEstimatedBiasFy());
25020 assertNull(calibrator.getEstimatedBiasFz());
25021 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25022 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25023 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25024 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25025 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25026 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25027 assertNull(calibrator.getEstimatedMa());
25028 assertNull(calibrator.getEstimatedSx());
25029 assertNull(calibrator.getEstimatedSy());
25030 assertNull(calibrator.getEstimatedSz());
25031 assertNull(calibrator.getEstimatedMxy());
25032 assertNull(calibrator.getEstimatedMxz());
25033 assertNull(calibrator.getEstimatedMyx());
25034 assertNull(calibrator.getEstimatedMyz());
25035 assertNull(calibrator.getEstimatedMzx());
25036 assertNull(calibrator.getEstimatedMzy());
25037 assertNull(calibrator.getEstimatedCovariance());
25038 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25039 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25040 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25041 assertNotNull(calibrator.getGroundTruthGravityNorm());
25042 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25043 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25044 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25045 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25046 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25047 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25048 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25049 }
25050
25051 @Test
25052 public void testConstructor196() throws WrongSizeException {
25053 final Matrix ba = generateBa();
25054 final double biasX = ba.getElementAtIndex(0);
25055 final double biasY = ba.getElementAtIndex(1);
25056 final double biasZ = ba.getElementAtIndex(2);
25057
25058 final Matrix ma = generateMaCommonAxis();
25059 final double sx = ma.getElementAt(0, 0);
25060 final double sy = ma.getElementAt(1, 1);
25061 final double sz = ma.getElementAt(2, 2);
25062 final double mxy = ma.getElementAt(0, 1);
25063 final double mxz = ma.getElementAt(0, 2);
25064 final double myx = ma.getElementAt(1, 0);
25065 final double myz = ma.getElementAt(1, 2);
25066 final double mzx = ma.getElementAt(2, 0);
25067 final double mzy = ma.getElementAt(2, 1);
25068
25069 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25070 final double latitude = Math.toRadians(
25071 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25072 final double longitude = Math.toRadians(
25073 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25074 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25075 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25076 final NEDVelocity nedVelocity = new NEDVelocity();
25077 final ECEFPosition ecefPosition = new ECEFPosition();
25078 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25079 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25080 ecefPosition, ecefVelocity);
25081
25082 final KnownPositionAccelerometerCalibrator calibrator =
25083 new KnownPositionAccelerometerCalibrator(nedPosition,
25084 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25085 myx, myz, mzx, mzy);
25086
25087
25088 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25089 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25090 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25091 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25092 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25093 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25094 final Acceleration bx2 = new Acceleration(0.0,
25095 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25096 calibrator.getInitialBiasXAsAcceleration(bx2);
25097 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25098 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25099 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25100 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25101 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25102 final Acceleration by2 = new Acceleration(0.0,
25103 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25104 calibrator.getInitialBiasYAsAcceleration(by2);
25105 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25106 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25107 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25108 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25109 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25110 final Acceleration bz2 = new Acceleration(0.0,
25111 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25112 calibrator.getInitialBiasZAsAcceleration(bz2);
25113 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25114 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25115 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25116 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25117 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25118 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25119 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25120 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25121 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25122 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25123 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25124 final double[] bias1 = calibrator.getInitialBias();
25125 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25126 final double[] bias2 = new double[3];
25127 calibrator.getInitialBias(bias2);
25128 assertArrayEquals(bias1, bias2, 0.0);
25129 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25130 assertEquals(b1, ba);
25131 final Matrix b2 = new Matrix(3, 1);
25132 calibrator.getInitialBiasAsMatrix(b2);
25133 assertEquals(b1, b2);
25134 final Matrix ma1 = new Matrix(3, 3);
25135 ma1.setSubmatrix(0, 0,
25136 2, 2,
25137 new double[]{sx, myx, mzx,
25138 mxy, sy, mzy,
25139 mxz, myz, sz});
25140 assertEquals(calibrator.getInitialMa(), ma1);
25141 final Matrix ma2 = new Matrix(3, 3);
25142 calibrator.getInitialMa(ma2);
25143 assertEquals(ma1, ma2);
25144 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25145 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25146 final NEDPosition nedPosition1 = new NEDPosition();
25147 assertTrue(calibrator.getNedPosition(nedPosition1));
25148 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25149 assertNull(calibrator.getMeasurements());
25150 assertFalse(calibrator.isCommonAxisUsed());
25151 assertNull(calibrator.getListener());
25152 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25153 assertFalse(calibrator.isReady());
25154 assertFalse(calibrator.isRunning());
25155 assertNull(calibrator.getEstimatedBiases());
25156 assertFalse(calibrator.getEstimatedBiases(null));
25157 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25158 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25159 assertNull(calibrator.getEstimatedBiasFx());
25160 assertNull(calibrator.getEstimatedBiasFy());
25161 assertNull(calibrator.getEstimatedBiasFz());
25162 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25163 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25164 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25165 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25166 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25167 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25168 assertNull(calibrator.getEstimatedMa());
25169 assertNull(calibrator.getEstimatedSx());
25170 assertNull(calibrator.getEstimatedSy());
25171 assertNull(calibrator.getEstimatedSz());
25172 assertNull(calibrator.getEstimatedMxy());
25173 assertNull(calibrator.getEstimatedMxz());
25174 assertNull(calibrator.getEstimatedMyx());
25175 assertNull(calibrator.getEstimatedMyz());
25176 assertNull(calibrator.getEstimatedMzx());
25177 assertNull(calibrator.getEstimatedMzy());
25178 assertNull(calibrator.getEstimatedCovariance());
25179 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25180 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25181 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25182 assertNotNull(calibrator.getGroundTruthGravityNorm());
25183 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25184 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25185 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25186 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25187 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25188 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25189 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25190 }
25191
25192 @Test
25193 public void testConstructor197() throws WrongSizeException {
25194 final Collection<StandardDeviationBodyKinematics> measurements =
25195 Collections.emptyList();
25196
25197 final Matrix ba = generateBa();
25198 final double biasX = ba.getElementAtIndex(0);
25199 final double biasY = ba.getElementAtIndex(1);
25200 final double biasZ = ba.getElementAtIndex(2);
25201
25202 final Matrix ma = generateMaCommonAxis();
25203 final double sx = ma.getElementAt(0, 0);
25204 final double sy = ma.getElementAt(1, 1);
25205 final double sz = ma.getElementAt(2, 2);
25206 final double mxy = ma.getElementAt(0, 1);
25207 final double mxz = ma.getElementAt(0, 2);
25208 final double myx = ma.getElementAt(1, 0);
25209 final double myz = ma.getElementAt(1, 2);
25210 final double mzx = ma.getElementAt(2, 0);
25211 final double mzy = ma.getElementAt(2, 1);
25212
25213 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25214 final double latitude = Math.toRadians(
25215 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25216 final double longitude = Math.toRadians(
25217 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25218 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25219 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25220 final NEDVelocity nedVelocity = new NEDVelocity();
25221 final ECEFPosition ecefPosition = new ECEFPosition();
25222 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25223 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25224 ecefPosition, ecefVelocity);
25225
25226 final KnownPositionAccelerometerCalibrator calibrator =
25227 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25228 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25229 myx, myz, mzx, mzy);
25230
25231
25232 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25233 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25234 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25235 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25236 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25237 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25238 final Acceleration bx2 = new Acceleration(0.0,
25239 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25240 calibrator.getInitialBiasXAsAcceleration(bx2);
25241 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25242 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25243 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25244 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25245 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25246 final Acceleration by2 = new Acceleration(0.0,
25247 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25248 calibrator.getInitialBiasYAsAcceleration(by2);
25249 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25250 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25251 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25252 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25253 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25254 final Acceleration bz2 = new Acceleration(0.0,
25255 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25256 calibrator.getInitialBiasZAsAcceleration(bz2);
25257 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25258 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25259 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25260 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25261 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25262 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25263 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25264 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25265 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25266 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25267 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25268 final double[] bias1 = calibrator.getInitialBias();
25269 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25270 final double[] bias2 = new double[3];
25271 calibrator.getInitialBias(bias2);
25272 assertArrayEquals(bias1, bias2, 0.0);
25273 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25274 assertEquals(b1, ba);
25275 final Matrix b2 = new Matrix(3, 1);
25276 calibrator.getInitialBiasAsMatrix(b2);
25277 assertEquals(b1, b2);
25278 final Matrix ma1 = new Matrix(3, 3);
25279 ma1.setSubmatrix(0, 0,
25280 2, 2,
25281 new double[]{sx, myx, mzx,
25282 mxy, sy, mzy,
25283 mxz, myz, sz});
25284 assertEquals(calibrator.getInitialMa(), ma1);
25285 final Matrix ma2 = new Matrix(3, 3);
25286 calibrator.getInitialMa(ma2);
25287 assertEquals(ma1, ma2);
25288 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25289 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25290 final NEDPosition nedPosition1 = new NEDPosition();
25291 assertTrue(calibrator.getNedPosition(nedPosition1));
25292 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25293 assertSame(calibrator.getMeasurements(), measurements);
25294 assertFalse(calibrator.isCommonAxisUsed());
25295 assertNull(calibrator.getListener());
25296 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25297 assertFalse(calibrator.isReady());
25298 assertFalse(calibrator.isRunning());
25299 assertNull(calibrator.getEstimatedBiases());
25300 assertFalse(calibrator.getEstimatedBiases(null));
25301 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25302 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25303 assertNull(calibrator.getEstimatedBiasFx());
25304 assertNull(calibrator.getEstimatedBiasFy());
25305 assertNull(calibrator.getEstimatedBiasFz());
25306 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25307 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25308 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25309 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25310 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25311 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25312 assertNull(calibrator.getEstimatedMa());
25313 assertNull(calibrator.getEstimatedSx());
25314 assertNull(calibrator.getEstimatedSy());
25315 assertNull(calibrator.getEstimatedSz());
25316 assertNull(calibrator.getEstimatedMxy());
25317 assertNull(calibrator.getEstimatedMxz());
25318 assertNull(calibrator.getEstimatedMyx());
25319 assertNull(calibrator.getEstimatedMyz());
25320 assertNull(calibrator.getEstimatedMzx());
25321 assertNull(calibrator.getEstimatedMzy());
25322 assertNull(calibrator.getEstimatedCovariance());
25323 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25324 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25325 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25326 assertNotNull(calibrator.getGroundTruthGravityNorm());
25327 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25328 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25329 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25330 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25331 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25332 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25333 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25334 }
25335
25336 @Test
25337 public void testConstructor198() throws WrongSizeException {
25338 final Collection<StandardDeviationBodyKinematics> measurements =
25339 Collections.emptyList();
25340
25341 final Matrix ba = generateBa();
25342 final double biasX = ba.getElementAtIndex(0);
25343 final double biasY = ba.getElementAtIndex(1);
25344 final double biasZ = ba.getElementAtIndex(2);
25345
25346 final Matrix ma = generateMaCommonAxis();
25347 final double sx = ma.getElementAt(0, 0);
25348 final double sy = ma.getElementAt(1, 1);
25349 final double sz = ma.getElementAt(2, 2);
25350 final double mxy = ma.getElementAt(0, 1);
25351 final double mxz = ma.getElementAt(0, 2);
25352 final double myx = ma.getElementAt(1, 0);
25353 final double myz = ma.getElementAt(1, 2);
25354 final double mzx = ma.getElementAt(2, 0);
25355 final double mzy = ma.getElementAt(2, 1);
25356
25357 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25358 final double latitude = Math.toRadians(
25359 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25360 final double longitude = Math.toRadians(
25361 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25362 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25363 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25364 final NEDVelocity nedVelocity = new NEDVelocity();
25365 final ECEFPosition ecefPosition = new ECEFPosition();
25366 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25367 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25368 ecefPosition, ecefVelocity);
25369
25370 final KnownPositionAccelerometerCalibrator calibrator =
25371 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25372 biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25373 myx, myz, mzx, mzy, this);
25374
25375
25376 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25377 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25378 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25379 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25380 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25381 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25382 final Acceleration bx2 = new Acceleration(0.0,
25383 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25384 calibrator.getInitialBiasXAsAcceleration(bx2);
25385 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25386 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25387 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25388 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25389 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25390 final Acceleration by2 = new Acceleration(0.0,
25391 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25392 calibrator.getInitialBiasYAsAcceleration(by2);
25393 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25394 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25395 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25396 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25397 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25398 final Acceleration bz2 = new Acceleration(0.0,
25399 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25400 calibrator.getInitialBiasZAsAcceleration(bz2);
25401 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25402 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25403 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25404 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25405 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25406 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25407 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25408 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25409 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25410 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25411 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25412 final double[] bias1 = calibrator.getInitialBias();
25413 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25414 final double[] bias2 = new double[3];
25415 calibrator.getInitialBias(bias2);
25416 assertArrayEquals(bias1, bias2, 0.0);
25417 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25418 assertEquals(b1, ba);
25419 final Matrix b2 = new Matrix(3, 1);
25420 calibrator.getInitialBiasAsMatrix(b2);
25421 assertEquals(b1, b2);
25422 final Matrix ma1 = new Matrix(3, 3);
25423 ma1.setSubmatrix(0, 0,
25424 2, 2,
25425 new double[]{sx, myx, mzx,
25426 mxy, sy, mzy,
25427 mxz, myz, sz});
25428 assertEquals(calibrator.getInitialMa(), ma1);
25429 final Matrix ma2 = new Matrix(3, 3);
25430 calibrator.getInitialMa(ma2);
25431 assertEquals(ma1, ma2);
25432 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25433 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25434 final NEDPosition nedPosition1 = new NEDPosition();
25435 assertTrue(calibrator.getNedPosition(nedPosition1));
25436 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25437 assertSame(calibrator.getMeasurements(), measurements);
25438 assertFalse(calibrator.isCommonAxisUsed());
25439 assertSame(calibrator.getListener(), this);
25440 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
25441 assertFalse(calibrator.isReady());
25442 assertFalse(calibrator.isRunning());
25443 assertNull(calibrator.getEstimatedBiases());
25444 assertFalse(calibrator.getEstimatedBiases(null));
25445 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25446 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25447 assertNull(calibrator.getEstimatedBiasFx());
25448 assertNull(calibrator.getEstimatedBiasFy());
25449 assertNull(calibrator.getEstimatedBiasFz());
25450 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25451 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25452 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25453 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25454 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25455 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25456 assertNull(calibrator.getEstimatedMa());
25457 assertNull(calibrator.getEstimatedSx());
25458 assertNull(calibrator.getEstimatedSy());
25459 assertNull(calibrator.getEstimatedSz());
25460 assertNull(calibrator.getEstimatedMxy());
25461 assertNull(calibrator.getEstimatedMxz());
25462 assertNull(calibrator.getEstimatedMyx());
25463 assertNull(calibrator.getEstimatedMyz());
25464 assertNull(calibrator.getEstimatedMzx());
25465 assertNull(calibrator.getEstimatedMzy());
25466 assertNull(calibrator.getEstimatedCovariance());
25467 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25468 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25469 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25470 assertNotNull(calibrator.getGroundTruthGravityNorm());
25471 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25472 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25473 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25474 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25475 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25476 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25477 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25478 }
25479
25480 @Test
25481 public void testConstructor199() throws WrongSizeException {
25482 final Matrix ba = generateBa();
25483 final double biasX = ba.getElementAtIndex(0);
25484 final double biasY = ba.getElementAtIndex(1);
25485 final double biasZ = ba.getElementAtIndex(2);
25486
25487 final Matrix ma = generateMaCommonAxis();
25488 final double sx = ma.getElementAt(0, 0);
25489 final double sy = ma.getElementAt(1, 1);
25490 final double sz = ma.getElementAt(2, 2);
25491 final double mxy = ma.getElementAt(0, 1);
25492 final double mxz = ma.getElementAt(0, 2);
25493 final double myx = ma.getElementAt(1, 0);
25494 final double myz = ma.getElementAt(1, 2);
25495 final double mzx = ma.getElementAt(2, 0);
25496 final double mzy = ma.getElementAt(2, 1);
25497
25498 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25499 final double latitude = Math.toRadians(
25500 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25501 final double longitude = Math.toRadians(
25502 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25503 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25504 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25505 final NEDVelocity nedVelocity = new NEDVelocity();
25506 final ECEFPosition ecefPosition = new ECEFPosition();
25507 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25508 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25509 ecefPosition, ecefVelocity);
25510
25511 final KnownPositionAccelerometerCalibrator calibrator =
25512 new KnownPositionAccelerometerCalibrator(nedPosition,
25513 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25514 myx, myz, mzx, mzy);
25515
25516
25517 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25518 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25519 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25520 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25521 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25522 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25523 final Acceleration bx2 = new Acceleration(0.0,
25524 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25525 calibrator.getInitialBiasXAsAcceleration(bx2);
25526 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25527 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25528 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25529 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25530 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25531 final Acceleration by2 = new Acceleration(0.0,
25532 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25533 calibrator.getInitialBiasYAsAcceleration(by2);
25534 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25535 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25536 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25537 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25538 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25539 final Acceleration bz2 = new Acceleration(0.0,
25540 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25541 calibrator.getInitialBiasZAsAcceleration(bz2);
25542 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25543 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25544 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25545 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25546 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25547 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25548 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25549 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25550 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25551 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25552 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25553 final double[] bias1 = calibrator.getInitialBias();
25554 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25555 final double[] bias2 = new double[3];
25556 calibrator.getInitialBias(bias2);
25557 assertArrayEquals(bias1, bias2, 0.0);
25558 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25559 assertEquals(b1, ba);
25560 final Matrix b2 = new Matrix(3, 1);
25561 calibrator.getInitialBiasAsMatrix(b2);
25562 assertEquals(b1, b2);
25563 final Matrix ma1 = new Matrix(3, 3);
25564 ma1.setSubmatrix(0, 0,
25565 2, 2,
25566 new double[]{sx, myx, mzx,
25567 mxy, sy, mzy,
25568 mxz, myz, sz});
25569 assertEquals(calibrator.getInitialMa(), ma1);
25570 final Matrix ma2 = new Matrix(3, 3);
25571 calibrator.getInitialMa(ma2);
25572 assertEquals(ma1, ma2);
25573 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25574 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25575 final NEDPosition nedPosition1 = new NEDPosition();
25576 assertTrue(calibrator.getNedPosition(nedPosition1));
25577 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25578 assertNull(calibrator.getMeasurements());
25579 assertTrue(calibrator.isCommonAxisUsed());
25580 assertNull(calibrator.getListener());
25581 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25582 assertFalse(calibrator.isReady());
25583 assertFalse(calibrator.isRunning());
25584 assertNull(calibrator.getEstimatedBiases());
25585 assertFalse(calibrator.getEstimatedBiases(null));
25586 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25587 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25588 assertNull(calibrator.getEstimatedBiasFx());
25589 assertNull(calibrator.getEstimatedBiasFy());
25590 assertNull(calibrator.getEstimatedBiasFz());
25591 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25592 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25593 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25594 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25595 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25596 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25597 assertNull(calibrator.getEstimatedMa());
25598 assertNull(calibrator.getEstimatedSx());
25599 assertNull(calibrator.getEstimatedSy());
25600 assertNull(calibrator.getEstimatedSz());
25601 assertNull(calibrator.getEstimatedMxy());
25602 assertNull(calibrator.getEstimatedMxz());
25603 assertNull(calibrator.getEstimatedMyx());
25604 assertNull(calibrator.getEstimatedMyz());
25605 assertNull(calibrator.getEstimatedMzx());
25606 assertNull(calibrator.getEstimatedMzy());
25607 assertNull(calibrator.getEstimatedCovariance());
25608 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25609 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25610 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25611 assertNotNull(calibrator.getGroundTruthGravityNorm());
25612 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25613 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25614 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25615 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25616 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25617 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25618 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25619 }
25620
25621 @Test
25622 public void testConstructor200() throws WrongSizeException {
25623 final Matrix ba = generateBa();
25624 final double biasX = ba.getElementAtIndex(0);
25625 final double biasY = ba.getElementAtIndex(1);
25626 final double biasZ = ba.getElementAtIndex(2);
25627
25628 final Matrix ma = generateMaCommonAxis();
25629 final double sx = ma.getElementAt(0, 0);
25630 final double sy = ma.getElementAt(1, 1);
25631 final double sz = ma.getElementAt(2, 2);
25632 final double mxy = ma.getElementAt(0, 1);
25633 final double mxz = ma.getElementAt(0, 2);
25634 final double myx = ma.getElementAt(1, 0);
25635 final double myz = ma.getElementAt(1, 2);
25636 final double mzx = ma.getElementAt(2, 0);
25637 final double mzy = ma.getElementAt(2, 1);
25638
25639 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25640 final double latitude = Math.toRadians(
25641 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25642 final double longitude = Math.toRadians(
25643 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25644 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25645 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25646 final NEDVelocity nedVelocity = new NEDVelocity();
25647 final ECEFPosition ecefPosition = new ECEFPosition();
25648 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25649 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25650 ecefPosition, ecefVelocity);
25651
25652 final KnownPositionAccelerometerCalibrator calibrator =
25653 new KnownPositionAccelerometerCalibrator(nedPosition,
25654 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25655 myx, myz, mzx, mzy, this);
25656
25657
25658 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25659 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25660 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25661 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25662 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25663 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25664 final Acceleration bx2 = new Acceleration(0.0,
25665 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25666 calibrator.getInitialBiasXAsAcceleration(bx2);
25667 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25668 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25669 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25670 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25671 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25672 final Acceleration by2 = new Acceleration(0.0,
25673 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25674 calibrator.getInitialBiasYAsAcceleration(by2);
25675 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25676 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25677 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25678 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25679 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25680 final Acceleration bz2 = new Acceleration(0.0,
25681 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25682 calibrator.getInitialBiasZAsAcceleration(bz2);
25683 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25684 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25685 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25686 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25687 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25688 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25689 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25690 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25691 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25692 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25693 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25694 final double[] bias1 = calibrator.getInitialBias();
25695 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25696 final double[] bias2 = new double[3];
25697 calibrator.getInitialBias(bias2);
25698 assertArrayEquals(bias1, bias2, 0.0);
25699 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25700 assertEquals(b1, ba);
25701 final Matrix b2 = new Matrix(3, 1);
25702 calibrator.getInitialBiasAsMatrix(b2);
25703 assertEquals(b1, b2);
25704 final Matrix ma1 = new Matrix(3, 3);
25705 ma1.setSubmatrix(0, 0,
25706 2, 2,
25707 new double[]{sx, myx, mzx,
25708 mxy, sy, mzy,
25709 mxz, myz, sz});
25710 assertEquals(calibrator.getInitialMa(), ma1);
25711 final Matrix ma2 = new Matrix(3, 3);
25712 calibrator.getInitialMa(ma2);
25713 assertEquals(ma1, ma2);
25714 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25715 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25716 final NEDPosition nedPosition1 = new NEDPosition();
25717 assertTrue(calibrator.getNedPosition(nedPosition1));
25718 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25719 assertNull(calibrator.getMeasurements());
25720 assertTrue(calibrator.isCommonAxisUsed());
25721 assertSame(calibrator.getListener(), this);
25722 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25723 assertFalse(calibrator.isReady());
25724 assertFalse(calibrator.isRunning());
25725 assertNull(calibrator.getEstimatedBiases());
25726 assertFalse(calibrator.getEstimatedBiases(null));
25727 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25728 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25729 assertNull(calibrator.getEstimatedBiasFx());
25730 assertNull(calibrator.getEstimatedBiasFy());
25731 assertNull(calibrator.getEstimatedBiasFz());
25732 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25733 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25734 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25735 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25736 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25737 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25738 assertNull(calibrator.getEstimatedMa());
25739 assertNull(calibrator.getEstimatedSx());
25740 assertNull(calibrator.getEstimatedSy());
25741 assertNull(calibrator.getEstimatedSz());
25742 assertNull(calibrator.getEstimatedMxy());
25743 assertNull(calibrator.getEstimatedMxz());
25744 assertNull(calibrator.getEstimatedMyx());
25745 assertNull(calibrator.getEstimatedMyz());
25746 assertNull(calibrator.getEstimatedMzx());
25747 assertNull(calibrator.getEstimatedMzy());
25748 assertNull(calibrator.getEstimatedCovariance());
25749 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25750 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25751 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25752 assertNotNull(calibrator.getGroundTruthGravityNorm());
25753 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25754 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25755 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25756 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25757 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25758 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25759 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25760 }
25761
25762 @Test
25763 public void testConstructor201() throws WrongSizeException {
25764 final Collection<StandardDeviationBodyKinematics> measurements =
25765 Collections.emptyList();
25766
25767 final Matrix ba = generateBa();
25768 final double biasX = ba.getElementAtIndex(0);
25769 final double biasY = ba.getElementAtIndex(1);
25770 final double biasZ = ba.getElementAtIndex(2);
25771
25772 final Matrix ma = generateMaCommonAxis();
25773 final double sx = ma.getElementAt(0, 0);
25774 final double sy = ma.getElementAt(1, 1);
25775 final double sz = ma.getElementAt(2, 2);
25776 final double mxy = ma.getElementAt(0, 1);
25777 final double mxz = ma.getElementAt(0, 2);
25778 final double myx = ma.getElementAt(1, 0);
25779 final double myz = ma.getElementAt(1, 2);
25780 final double mzx = ma.getElementAt(2, 0);
25781 final double mzy = ma.getElementAt(2, 1);
25782
25783 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25784 final double latitude = Math.toRadians(
25785 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25786 final double longitude = Math.toRadians(
25787 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25788 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25789 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25790 final NEDVelocity nedVelocity = new NEDVelocity();
25791 final ECEFPosition ecefPosition = new ECEFPosition();
25792 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25793 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25794 ecefPosition, ecefVelocity);
25795
25796 final KnownPositionAccelerometerCalibrator calibrator =
25797 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25798 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25799 myx, myz, mzx, mzy);
25800
25801
25802 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25803 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25804 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25805 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25806 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25807 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25808 final Acceleration bx2 = new Acceleration(0.0,
25809 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25810 calibrator.getInitialBiasXAsAcceleration(bx2);
25811 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25812 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25813 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25814 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25815 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25816 final Acceleration by2 = new Acceleration(0.0,
25817 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25818 calibrator.getInitialBiasYAsAcceleration(by2);
25819 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25820 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25821 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25822 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25823 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25824 final Acceleration bz2 = new Acceleration(0.0,
25825 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25826 calibrator.getInitialBiasZAsAcceleration(bz2);
25827 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25828 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25829 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25830 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25831 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25832 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25833 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25834 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25835 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25836 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25837 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25838 final double[] bias1 = calibrator.getInitialBias();
25839 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25840 final double[] bias2 = new double[3];
25841 calibrator.getInitialBias(bias2);
25842 assertArrayEquals(bias1, bias2, 0.0);
25843 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25844 assertEquals(b1, ba);
25845 final Matrix b2 = new Matrix(3, 1);
25846 calibrator.getInitialBiasAsMatrix(b2);
25847 assertEquals(b1, b2);
25848 final Matrix ma1 = new Matrix(3, 3);
25849 ma1.setSubmatrix(0, 0,
25850 2, 2,
25851 new double[]{sx, myx, mzx,
25852 mxy, sy, mzy,
25853 mxz, myz, sz});
25854 assertEquals(calibrator.getInitialMa(), ma1);
25855 final Matrix ma2 = new Matrix(3, 3);
25856 calibrator.getInitialMa(ma2);
25857 assertEquals(ma1, ma2);
25858 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
25859 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
25860 final NEDPosition nedPosition1 = new NEDPosition();
25861 assertTrue(calibrator.getNedPosition(nedPosition1));
25862 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
25863 assertSame(calibrator.getMeasurements(), measurements);
25864 assertTrue(calibrator.isCommonAxisUsed());
25865 assertNull(calibrator.getListener());
25866 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
25867 assertFalse(calibrator.isReady());
25868 assertFalse(calibrator.isRunning());
25869 assertNull(calibrator.getEstimatedBiases());
25870 assertFalse(calibrator.getEstimatedBiases(null));
25871 assertNull(calibrator.getEstimatedBiasesAsMatrix());
25872 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
25873 assertNull(calibrator.getEstimatedBiasFx());
25874 assertNull(calibrator.getEstimatedBiasFy());
25875 assertNull(calibrator.getEstimatedBiasFz());
25876 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
25877 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
25878 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
25879 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
25880 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
25881 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
25882 assertNull(calibrator.getEstimatedMa());
25883 assertNull(calibrator.getEstimatedSx());
25884 assertNull(calibrator.getEstimatedSy());
25885 assertNull(calibrator.getEstimatedSz());
25886 assertNull(calibrator.getEstimatedMxy());
25887 assertNull(calibrator.getEstimatedMxz());
25888 assertNull(calibrator.getEstimatedMyx());
25889 assertNull(calibrator.getEstimatedMyz());
25890 assertNull(calibrator.getEstimatedMzx());
25891 assertNull(calibrator.getEstimatedMzy());
25892 assertNull(calibrator.getEstimatedCovariance());
25893 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
25894 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
25895 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
25896 assertNotNull(calibrator.getGroundTruthGravityNorm());
25897 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
25898 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
25899 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
25900 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
25901 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
25902 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
25903 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
25904 }
25905
25906 @Test
25907 public void testConstructor202() throws WrongSizeException {
25908 final Collection<StandardDeviationBodyKinematics> measurements =
25909 Collections.emptyList();
25910
25911 final Matrix ba = generateBa();
25912 final double biasX = ba.getElementAtIndex(0);
25913 final double biasY = ba.getElementAtIndex(1);
25914 final double biasZ = ba.getElementAtIndex(2);
25915
25916 final Matrix ma = generateMaCommonAxis();
25917 final double sx = ma.getElementAt(0, 0);
25918 final double sy = ma.getElementAt(1, 1);
25919 final double sz = ma.getElementAt(2, 2);
25920 final double mxy = ma.getElementAt(0, 1);
25921 final double mxz = ma.getElementAt(0, 2);
25922 final double myx = ma.getElementAt(1, 0);
25923 final double myz = ma.getElementAt(1, 2);
25924 final double mzx = ma.getElementAt(2, 0);
25925 final double mzy = ma.getElementAt(2, 1);
25926
25927 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
25928 final double latitude = Math.toRadians(
25929 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
25930 final double longitude = Math.toRadians(
25931 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
25932 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
25933 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
25934 final NEDVelocity nedVelocity = new NEDVelocity();
25935 final ECEFPosition ecefPosition = new ECEFPosition();
25936 final ECEFVelocity ecefVelocity = new ECEFVelocity();
25937 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
25938 ecefPosition, ecefVelocity);
25939
25940 final KnownPositionAccelerometerCalibrator calibrator =
25941 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
25942 true, biasX, biasY, biasZ, sx, sy, sz, mxy, mxz,
25943 myx, myz, mzx, mzy, this);
25944
25945
25946 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
25947 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
25948 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
25949 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
25950 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
25951 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25952 final Acceleration bx2 = new Acceleration(0.0,
25953 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25954 calibrator.getInitialBiasXAsAcceleration(bx2);
25955 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
25956 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25957 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
25958 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
25959 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25960 final Acceleration by2 = new Acceleration(0.0,
25961 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25962 calibrator.getInitialBiasYAsAcceleration(by2);
25963 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
25964 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25965 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
25966 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
25967 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25968 final Acceleration bz2 = new Acceleration(0.0,
25969 AccelerationUnit.FEET_PER_SQUARED_SECOND);
25970 calibrator.getInitialBiasZAsAcceleration(bz2);
25971 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
25972 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
25973 assertEquals(calibrator.getInitialSx(), sx, 0.0);
25974 assertEquals(calibrator.getInitialSy(), sy, 0.0);
25975 assertEquals(calibrator.getInitialSz(), sz, 0.0);
25976 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
25977 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
25978 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
25979 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
25980 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
25981 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
25982 final double[] bias1 = calibrator.getInitialBias();
25983 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
25984 final double[] bias2 = new double[3];
25985 calibrator.getInitialBias(bias2);
25986 assertArrayEquals(bias1, bias2, 0.0);
25987 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
25988 assertEquals(b1, ba);
25989 final Matrix b2 = new Matrix(3, 1);
25990 calibrator.getInitialBiasAsMatrix(b2);
25991 assertEquals(b1, b2);
25992 final Matrix ma1 = new Matrix(3, 3);
25993 ma1.setSubmatrix(0, 0,
25994 2, 2,
25995 new double[]{sx, myx, mzx,
25996 mxy, sy, mzy,
25997 mxz, myz, sz});
25998 assertEquals(calibrator.getInitialMa(), ma1);
25999 final Matrix ma2 = new Matrix(3, 3);
26000 calibrator.getInitialMa(ma2);
26001 assertEquals(ma1, ma2);
26002 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26003 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26004 final NEDPosition nedPosition1 = new NEDPosition();
26005 assertTrue(calibrator.getNedPosition(nedPosition1));
26006 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26007 assertSame(calibrator.getMeasurements(), measurements);
26008 assertTrue(calibrator.isCommonAxisUsed());
26009 assertSame(calibrator.getListener(), this);
26010 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26011 assertFalse(calibrator.isReady());
26012 assertFalse(calibrator.isRunning());
26013 assertNull(calibrator.getEstimatedBiases());
26014 assertFalse(calibrator.getEstimatedBiases(null));
26015 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26016 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26017 assertNull(calibrator.getEstimatedBiasFx());
26018 assertNull(calibrator.getEstimatedBiasFy());
26019 assertNull(calibrator.getEstimatedBiasFz());
26020 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26021 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26022 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26023 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26024 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26025 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26026 assertNull(calibrator.getEstimatedMa());
26027 assertNull(calibrator.getEstimatedSx());
26028 assertNull(calibrator.getEstimatedSy());
26029 assertNull(calibrator.getEstimatedSz());
26030 assertNull(calibrator.getEstimatedMxy());
26031 assertNull(calibrator.getEstimatedMxz());
26032 assertNull(calibrator.getEstimatedMyx());
26033 assertNull(calibrator.getEstimatedMyz());
26034 assertNull(calibrator.getEstimatedMzx());
26035 assertNull(calibrator.getEstimatedMzy());
26036 assertNull(calibrator.getEstimatedCovariance());
26037 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26038 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26039 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26040 assertNotNull(calibrator.getGroundTruthGravityNorm());
26041 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26042 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26043 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26044 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26045 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26046 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26047 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26048 }
26049
26050 @Test
26051 public void testConstructor203() throws WrongSizeException {
26052 final Matrix ba = generateBa();
26053 final double biasX = ba.getElementAtIndex(0);
26054 final double biasY = ba.getElementAtIndex(1);
26055 final double biasZ = ba.getElementAtIndex(2);
26056
26057 final Acceleration bx = new Acceleration(biasX,
26058 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26059 final Acceleration by = new Acceleration(biasY,
26060 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26061 final Acceleration bz = new Acceleration(biasZ,
26062 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26063
26064 final Matrix ma = generateMaCommonAxis();
26065 final double sx = ma.getElementAt(0, 0);
26066 final double sy = ma.getElementAt(1, 1);
26067 final double sz = ma.getElementAt(2, 2);
26068 final double mxy = ma.getElementAt(0, 1);
26069 final double mxz = ma.getElementAt(0, 2);
26070 final double myx = ma.getElementAt(1, 0);
26071 final double myz = ma.getElementAt(1, 2);
26072 final double mzx = ma.getElementAt(2, 0);
26073 final double mzy = ma.getElementAt(2, 1);
26074
26075 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26076 final double latitude = Math.toRadians(
26077 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26078 final double longitude = Math.toRadians(
26079 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26080 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26081 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26082 final NEDVelocity nedVelocity = new NEDVelocity();
26083 final ECEFPosition ecefPosition = new ECEFPosition();
26084 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26085 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26086 ecefPosition, ecefVelocity);
26087
26088 final KnownPositionAccelerometerCalibrator calibrator =
26089 new KnownPositionAccelerometerCalibrator(nedPosition,
26090 bx, by, bz, sx, sy, sz, mxy, mxz,
26091 myx, myz, mzx, mzy);
26092
26093
26094 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26095 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26096 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26097 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26098 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26099 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26100 final Acceleration bx2 = new Acceleration(0.0,
26101 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26102 calibrator.getInitialBiasXAsAcceleration(bx2);
26103 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26104 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26105 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26106 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26107 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26108 final Acceleration by2 = new Acceleration(0.0,
26109 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26110 calibrator.getInitialBiasYAsAcceleration(by2);
26111 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26112 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26113 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26114 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26115 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26116 final Acceleration bz2 = new Acceleration(0.0,
26117 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26118 calibrator.getInitialBiasZAsAcceleration(bz2);
26119 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26120 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26121 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26122 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26123 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26124 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26125 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26126 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26127 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26128 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26129 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26130 final double[] bias1 = calibrator.getInitialBias();
26131 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26132 final double[] bias2 = new double[3];
26133 calibrator.getInitialBias(bias2);
26134 assertArrayEquals(bias1, bias2, 0.0);
26135 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26136 assertEquals(b1, ba);
26137 final Matrix b2 = new Matrix(3, 1);
26138 calibrator.getInitialBiasAsMatrix(b2);
26139 assertEquals(b1, b2);
26140 final Matrix ma1 = new Matrix(3, 3);
26141 ma1.setSubmatrix(0, 0,
26142 2, 2,
26143 new double[]{sx, myx, mzx,
26144 mxy, sy, mzy,
26145 mxz, myz, sz});
26146 assertEquals(calibrator.getInitialMa(), ma1);
26147 final Matrix ma2 = new Matrix(3, 3);
26148 calibrator.getInitialMa(ma2);
26149 assertEquals(ma1, ma2);
26150 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26151 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26152 final NEDPosition nedPosition1 = new NEDPosition();
26153 assertTrue(calibrator.getNedPosition(nedPosition1));
26154 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26155 assertNull(calibrator.getMeasurements());
26156 assertFalse(calibrator.isCommonAxisUsed());
26157 assertNull(calibrator.getListener());
26158 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26159 assertFalse(calibrator.isReady());
26160 assertFalse(calibrator.isRunning());
26161 assertNull(calibrator.getEstimatedBiases());
26162 assertFalse(calibrator.getEstimatedBiases(null));
26163 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26164 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26165 assertNull(calibrator.getEstimatedBiasFx());
26166 assertNull(calibrator.getEstimatedBiasFy());
26167 assertNull(calibrator.getEstimatedBiasFz());
26168 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26169 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26170 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26171 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26172 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26173 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26174 assertNull(calibrator.getEstimatedMa());
26175 assertNull(calibrator.getEstimatedSx());
26176 assertNull(calibrator.getEstimatedSy());
26177 assertNull(calibrator.getEstimatedSz());
26178 assertNull(calibrator.getEstimatedMxy());
26179 assertNull(calibrator.getEstimatedMxz());
26180 assertNull(calibrator.getEstimatedMyx());
26181 assertNull(calibrator.getEstimatedMyz());
26182 assertNull(calibrator.getEstimatedMzx());
26183 assertNull(calibrator.getEstimatedMzy());
26184 assertNull(calibrator.getEstimatedCovariance());
26185 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26186 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26187 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26188 assertNotNull(calibrator.getGroundTruthGravityNorm());
26189 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26190 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26191 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26192 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26193 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26194 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26195 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26196 }
26197
26198 @Test
26199 public void testConstructor204() throws WrongSizeException {
26200 final Matrix ba = generateBa();
26201 final double biasX = ba.getElementAtIndex(0);
26202 final double biasY = ba.getElementAtIndex(1);
26203 final double biasZ = ba.getElementAtIndex(2);
26204
26205 final Acceleration bx = new Acceleration(biasX,
26206 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26207 final Acceleration by = new Acceleration(biasY,
26208 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26209 final Acceleration bz = new Acceleration(biasZ,
26210 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26211
26212 final Matrix ma = generateMaCommonAxis();
26213 final double sx = ma.getElementAt(0, 0);
26214 final double sy = ma.getElementAt(1, 1);
26215 final double sz = ma.getElementAt(2, 2);
26216 final double mxy = ma.getElementAt(0, 1);
26217 final double mxz = ma.getElementAt(0, 2);
26218 final double myx = ma.getElementAt(1, 0);
26219 final double myz = ma.getElementAt(1, 2);
26220 final double mzx = ma.getElementAt(2, 0);
26221 final double mzy = ma.getElementAt(2, 1);
26222
26223 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26224 final double latitude = Math.toRadians(
26225 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26226 final double longitude = Math.toRadians(
26227 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26228 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26229 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26230 final NEDVelocity nedVelocity = new NEDVelocity();
26231 final ECEFPosition ecefPosition = new ECEFPosition();
26232 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26233 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26234 ecefPosition, ecefVelocity);
26235
26236 final KnownPositionAccelerometerCalibrator calibrator =
26237 new KnownPositionAccelerometerCalibrator(nedPosition,
26238 bx, by, bz, sx, sy, sz, mxy, mxz,
26239 myx, myz, mzx, mzy, this);
26240
26241
26242 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26243 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26244 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26245 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26246 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26247 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26248 final Acceleration bx2 = new Acceleration(0.0,
26249 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26250 calibrator.getInitialBiasXAsAcceleration(bx2);
26251 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26252 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26253 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26254 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26255 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26256 final Acceleration by2 = new Acceleration(0.0,
26257 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26258 calibrator.getInitialBiasYAsAcceleration(by2);
26259 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26260 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26261 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26262 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26263 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26264 final Acceleration bz2 = new Acceleration(0.0,
26265 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26266 calibrator.getInitialBiasZAsAcceleration(bz2);
26267 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26268 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26269 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26270 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26271 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26272 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26273 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26274 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26275 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26276 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26277 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26278 final double[] bias1 = calibrator.getInitialBias();
26279 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26280 final double[] bias2 = new double[3];
26281 calibrator.getInitialBias(bias2);
26282 assertArrayEquals(bias1, bias2, 0.0);
26283 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26284 assertEquals(b1, ba);
26285 final Matrix b2 = new Matrix(3, 1);
26286 calibrator.getInitialBiasAsMatrix(b2);
26287 assertEquals(b1, b2);
26288 final Matrix ma1 = new Matrix(3, 3);
26289 ma1.setSubmatrix(0, 0,
26290 2, 2,
26291 new double[]{sx, myx, mzx,
26292 mxy, sy, mzy,
26293 mxz, myz, sz});
26294 assertEquals(calibrator.getInitialMa(), ma1);
26295 final Matrix ma2 = new Matrix(3, 3);
26296 calibrator.getInitialMa(ma2);
26297 assertEquals(ma1, ma2);
26298 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26299 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26300 final NEDPosition nedPosition1 = new NEDPosition();
26301 assertTrue(calibrator.getNedPosition(nedPosition1));
26302 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26303 assertNull(calibrator.getMeasurements());
26304 assertFalse(calibrator.isCommonAxisUsed());
26305 assertSame(calibrator.getListener(), this);
26306 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26307 assertFalse(calibrator.isReady());
26308 assertFalse(calibrator.isRunning());
26309 assertNull(calibrator.getEstimatedBiases());
26310 assertFalse(calibrator.getEstimatedBiases(null));
26311 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26312 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26313 assertNull(calibrator.getEstimatedBiasFx());
26314 assertNull(calibrator.getEstimatedBiasFy());
26315 assertNull(calibrator.getEstimatedBiasFz());
26316 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26317 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26318 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26319 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26320 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26321 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26322 assertNull(calibrator.getEstimatedMa());
26323 assertNull(calibrator.getEstimatedSx());
26324 assertNull(calibrator.getEstimatedSy());
26325 assertNull(calibrator.getEstimatedSz());
26326 assertNull(calibrator.getEstimatedMxy());
26327 assertNull(calibrator.getEstimatedMxz());
26328 assertNull(calibrator.getEstimatedMyx());
26329 assertNull(calibrator.getEstimatedMyz());
26330 assertNull(calibrator.getEstimatedMzx());
26331 assertNull(calibrator.getEstimatedMzy());
26332 assertNull(calibrator.getEstimatedCovariance());
26333 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26334 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26335 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26336 assertNotNull(calibrator.getGroundTruthGravityNorm());
26337 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26338 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26339 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26340 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26341 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26342 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26343 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26344 }
26345
26346 @Test
26347 public void testConstructor205() throws WrongSizeException {
26348 final Collection<StandardDeviationBodyKinematics> measurements =
26349 Collections.emptyList();
26350
26351 final Matrix ba = generateBa();
26352 final double biasX = ba.getElementAtIndex(0);
26353 final double biasY = ba.getElementAtIndex(1);
26354 final double biasZ = ba.getElementAtIndex(2);
26355
26356 final Acceleration bx = new Acceleration(biasX,
26357 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26358 final Acceleration by = new Acceleration(biasY,
26359 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26360 final Acceleration bz = new Acceleration(biasZ,
26361 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26362
26363 final Matrix ma = generateMaCommonAxis();
26364 final double sx = ma.getElementAt(0, 0);
26365 final double sy = ma.getElementAt(1, 1);
26366 final double sz = ma.getElementAt(2, 2);
26367 final double mxy = ma.getElementAt(0, 1);
26368 final double mxz = ma.getElementAt(0, 2);
26369 final double myx = ma.getElementAt(1, 0);
26370 final double myz = ma.getElementAt(1, 2);
26371 final double mzx = ma.getElementAt(2, 0);
26372 final double mzy = ma.getElementAt(2, 1);
26373
26374 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26375 final double latitude = Math.toRadians(
26376 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26377 final double longitude = Math.toRadians(
26378 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26379 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26380 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26381 final NEDVelocity nedVelocity = new NEDVelocity();
26382 final ECEFPosition ecefPosition = new ECEFPosition();
26383 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26384 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26385 ecefPosition, ecefVelocity);
26386
26387 final KnownPositionAccelerometerCalibrator calibrator =
26388 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26389 bx, by, bz, sx, sy, sz, mxy, mxz,
26390 myx, myz, mzx, mzy);
26391
26392
26393 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26394 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26395 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26396 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26397 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26398 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26399 final Acceleration bx2 = new Acceleration(0.0,
26400 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26401 calibrator.getInitialBiasXAsAcceleration(bx2);
26402 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26403 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26404 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26405 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26406 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26407 final Acceleration by2 = new Acceleration(0.0,
26408 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26409 calibrator.getInitialBiasYAsAcceleration(by2);
26410 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26411 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26412 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26413 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26414 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26415 final Acceleration bz2 = new Acceleration(0.0,
26416 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26417 calibrator.getInitialBiasZAsAcceleration(bz2);
26418 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26419 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26420 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26421 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26422 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26423 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26424 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26425 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26426 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26427 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26428 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26429 final double[] bias1 = calibrator.getInitialBias();
26430 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26431 final double[] bias2 = new double[3];
26432 calibrator.getInitialBias(bias2);
26433 assertArrayEquals(bias1, bias2, 0.0);
26434 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26435 assertEquals(b1, ba);
26436 final Matrix b2 = new Matrix(3, 1);
26437 calibrator.getInitialBiasAsMatrix(b2);
26438 assertEquals(b1, b2);
26439 final Matrix ma1 = new Matrix(3, 3);
26440 ma1.setSubmatrix(0, 0,
26441 2, 2,
26442 new double[]{sx, myx, mzx,
26443 mxy, sy, mzy,
26444 mxz, myz, sz});
26445 assertEquals(calibrator.getInitialMa(), ma1);
26446 final Matrix ma2 = new Matrix(3, 3);
26447 calibrator.getInitialMa(ma2);
26448 assertEquals(ma1, ma2);
26449 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26450 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26451 final NEDPosition nedPosition1 = new NEDPosition();
26452 assertTrue(calibrator.getNedPosition(nedPosition1));
26453 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26454 assertSame(calibrator.getMeasurements(), measurements);
26455 assertFalse(calibrator.isCommonAxisUsed());
26456 assertNull(calibrator.getListener());
26457 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26458 assertFalse(calibrator.isReady());
26459 assertFalse(calibrator.isRunning());
26460 assertNull(calibrator.getEstimatedBiases());
26461 assertFalse(calibrator.getEstimatedBiases(null));
26462 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26463 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26464 assertNull(calibrator.getEstimatedBiasFx());
26465 assertNull(calibrator.getEstimatedBiasFy());
26466 assertNull(calibrator.getEstimatedBiasFz());
26467 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26468 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26469 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26470 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26471 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26472 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26473 assertNull(calibrator.getEstimatedMa());
26474 assertNull(calibrator.getEstimatedSx());
26475 assertNull(calibrator.getEstimatedSy());
26476 assertNull(calibrator.getEstimatedSz());
26477 assertNull(calibrator.getEstimatedMxy());
26478 assertNull(calibrator.getEstimatedMxz());
26479 assertNull(calibrator.getEstimatedMyx());
26480 assertNull(calibrator.getEstimatedMyz());
26481 assertNull(calibrator.getEstimatedMzx());
26482 assertNull(calibrator.getEstimatedMzy());
26483 assertNull(calibrator.getEstimatedCovariance());
26484 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26485 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26486 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26487 assertNotNull(calibrator.getGroundTruthGravityNorm());
26488 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26489 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26490 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26491 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26492 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26493 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26494 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26495 }
26496
26497 @Test
26498 public void testConstructor206() throws WrongSizeException {
26499 final Collection<StandardDeviationBodyKinematics> measurements =
26500 Collections.emptyList();
26501
26502 final Matrix ba = generateBa();
26503 final double biasX = ba.getElementAtIndex(0);
26504 final double biasY = ba.getElementAtIndex(1);
26505 final double biasZ = ba.getElementAtIndex(2);
26506
26507 final Acceleration bx = new Acceleration(biasX,
26508 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26509 final Acceleration by = new Acceleration(biasY,
26510 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26511 final Acceleration bz = new Acceleration(biasZ,
26512 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26513
26514 final Matrix ma = generateMaCommonAxis();
26515 final double sx = ma.getElementAt(0, 0);
26516 final double sy = ma.getElementAt(1, 1);
26517 final double sz = ma.getElementAt(2, 2);
26518 final double mxy = ma.getElementAt(0, 1);
26519 final double mxz = ma.getElementAt(0, 2);
26520 final double myx = ma.getElementAt(1, 0);
26521 final double myz = ma.getElementAt(1, 2);
26522 final double mzx = ma.getElementAt(2, 0);
26523 final double mzy = ma.getElementAt(2, 1);
26524
26525 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26526 final double latitude = Math.toRadians(
26527 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26528 final double longitude = Math.toRadians(
26529 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26530 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26531 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26532 final NEDVelocity nedVelocity = new NEDVelocity();
26533 final ECEFPosition ecefPosition = new ECEFPosition();
26534 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26535 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26536 ecefPosition, ecefVelocity);
26537
26538 final KnownPositionAccelerometerCalibrator calibrator =
26539 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26540 bx, by, bz, sx, sy, sz, mxy, mxz,
26541 myx, myz, mzx, mzy, this);
26542
26543
26544 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26545 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26546 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26547 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26548 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26549 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26550 final Acceleration bx2 = new Acceleration(0.0,
26551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26552 calibrator.getInitialBiasXAsAcceleration(bx2);
26553 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26554 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26555 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26556 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26557 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26558 final Acceleration by2 = new Acceleration(0.0,
26559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26560 calibrator.getInitialBiasYAsAcceleration(by2);
26561 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26562 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26563 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26564 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26565 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26566 final Acceleration bz2 = new Acceleration(0.0,
26567 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26568 calibrator.getInitialBiasZAsAcceleration(bz2);
26569 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26570 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26571 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26572 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26573 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26574 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26575 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26576 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26577 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26578 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26579 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26580 final double[] bias1 = calibrator.getInitialBias();
26581 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26582 final double[] bias2 = new double[3];
26583 calibrator.getInitialBias(bias2);
26584 assertArrayEquals(bias1, bias2, 0.0);
26585 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26586 assertEquals(b1, ba);
26587 final Matrix b2 = new Matrix(3, 1);
26588 calibrator.getInitialBiasAsMatrix(b2);
26589 assertEquals(b1, b2);
26590 final Matrix ma1 = new Matrix(3, 3);
26591 ma1.setSubmatrix(0, 0,
26592 2, 2,
26593 new double[]{sx, myx, mzx,
26594 mxy, sy, mzy,
26595 mxz, myz, sz});
26596 assertEquals(calibrator.getInitialMa(), ma1);
26597 final Matrix ma2 = new Matrix(3, 3);
26598 calibrator.getInitialMa(ma2);
26599 assertEquals(ma1, ma2);
26600 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26601 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26602 final NEDPosition nedPosition1 = new NEDPosition();
26603 assertTrue(calibrator.getNedPosition(nedPosition1));
26604 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26605 assertSame(calibrator.getMeasurements(), measurements);
26606 assertFalse(calibrator.isCommonAxisUsed());
26607 assertSame(calibrator.getListener(), this);
26608 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
26609 assertFalse(calibrator.isReady());
26610 assertFalse(calibrator.isRunning());
26611 assertNull(calibrator.getEstimatedBiases());
26612 assertFalse(calibrator.getEstimatedBiases(null));
26613 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26614 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26615 assertNull(calibrator.getEstimatedBiasFx());
26616 assertNull(calibrator.getEstimatedBiasFy());
26617 assertNull(calibrator.getEstimatedBiasFz());
26618 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26619 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26620 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26621 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26622 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26623 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26624 assertNull(calibrator.getEstimatedMa());
26625 assertNull(calibrator.getEstimatedSx());
26626 assertNull(calibrator.getEstimatedSy());
26627 assertNull(calibrator.getEstimatedSz());
26628 assertNull(calibrator.getEstimatedMxy());
26629 assertNull(calibrator.getEstimatedMxz());
26630 assertNull(calibrator.getEstimatedMyx());
26631 assertNull(calibrator.getEstimatedMyz());
26632 assertNull(calibrator.getEstimatedMzx());
26633 assertNull(calibrator.getEstimatedMzy());
26634 assertNull(calibrator.getEstimatedCovariance());
26635 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26636 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26637 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26638 assertNotNull(calibrator.getGroundTruthGravityNorm());
26639 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26640 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26641 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26642 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26643 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26644 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26645 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26646 }
26647
26648 @Test
26649 public void testConstructor207() throws WrongSizeException {
26650 final Matrix ba = generateBa();
26651 final double biasX = ba.getElementAtIndex(0);
26652 final double biasY = ba.getElementAtIndex(1);
26653 final double biasZ = ba.getElementAtIndex(2);
26654
26655 final Acceleration bx = new Acceleration(biasX,
26656 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26657 final Acceleration by = new Acceleration(biasY,
26658 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26659 final Acceleration bz = new Acceleration(biasZ,
26660 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26661
26662 final Matrix ma = generateMaCommonAxis();
26663 final double sx = ma.getElementAt(0, 0);
26664 final double sy = ma.getElementAt(1, 1);
26665 final double sz = ma.getElementAt(2, 2);
26666 final double mxy = ma.getElementAt(0, 1);
26667 final double mxz = ma.getElementAt(0, 2);
26668 final double myx = ma.getElementAt(1, 0);
26669 final double myz = ma.getElementAt(1, 2);
26670 final double mzx = ma.getElementAt(2, 0);
26671 final double mzy = ma.getElementAt(2, 1);
26672
26673 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26674 final double latitude = Math.toRadians(
26675 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26676 final double longitude = Math.toRadians(
26677 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26678 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26679 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26680 final NEDVelocity nedVelocity = new NEDVelocity();
26681 final ECEFPosition ecefPosition = new ECEFPosition();
26682 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26683 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26684 ecefPosition, ecefVelocity);
26685
26686 final KnownPositionAccelerometerCalibrator calibrator =
26687 new KnownPositionAccelerometerCalibrator(nedPosition,
26688 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26689 myx, myz, mzx, mzy);
26690
26691
26692 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26693 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26694 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26695 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26696 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26697 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26698 final Acceleration bx2 = new Acceleration(0.0,
26699 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26700 calibrator.getInitialBiasXAsAcceleration(bx2);
26701 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26702 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26703 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26704 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26705 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26706 final Acceleration by2 = new Acceleration(0.0,
26707 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26708 calibrator.getInitialBiasYAsAcceleration(by2);
26709 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26710 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26711 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26712 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26713 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26714 final Acceleration bz2 = new Acceleration(0.0,
26715 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26716 calibrator.getInitialBiasZAsAcceleration(bz2);
26717 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26718 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26719 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26720 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26721 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26722 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26723 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26724 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26725 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26726 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26727 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26728 final double[] bias1 = calibrator.getInitialBias();
26729 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26730 final double[] bias2 = new double[3];
26731 calibrator.getInitialBias(bias2);
26732 assertArrayEquals(bias1, bias2, 0.0);
26733 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26734 assertEquals(b1, ba);
26735 final Matrix b2 = new Matrix(3, 1);
26736 calibrator.getInitialBiasAsMatrix(b2);
26737 assertEquals(b1, b2);
26738 final Matrix ma1 = new Matrix(3, 3);
26739 ma1.setSubmatrix(0, 0,
26740 2, 2,
26741 new double[]{sx, myx, mzx,
26742 mxy, sy, mzy,
26743 mxz, myz, sz});
26744 assertEquals(calibrator.getInitialMa(), ma1);
26745 final Matrix ma2 = new Matrix(3, 3);
26746 calibrator.getInitialMa(ma2);
26747 assertEquals(ma1, ma2);
26748 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26749 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26750 final NEDPosition nedPosition1 = new NEDPosition();
26751 assertTrue(calibrator.getNedPosition(nedPosition1));
26752 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26753 assertNull(calibrator.getMeasurements());
26754 assertTrue(calibrator.isCommonAxisUsed());
26755 assertNull(calibrator.getListener());
26756 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26757 assertFalse(calibrator.isReady());
26758 assertFalse(calibrator.isRunning());
26759 assertNull(calibrator.getEstimatedBiases());
26760 assertFalse(calibrator.getEstimatedBiases(null));
26761 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26762 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26763 assertNull(calibrator.getEstimatedBiasFx());
26764 assertNull(calibrator.getEstimatedBiasFy());
26765 assertNull(calibrator.getEstimatedBiasFz());
26766 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26767 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26768 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26769 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26770 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26771 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26772 assertNull(calibrator.getEstimatedMa());
26773 assertNull(calibrator.getEstimatedSx());
26774 assertNull(calibrator.getEstimatedSy());
26775 assertNull(calibrator.getEstimatedSz());
26776 assertNull(calibrator.getEstimatedMxy());
26777 assertNull(calibrator.getEstimatedMxz());
26778 assertNull(calibrator.getEstimatedMyx());
26779 assertNull(calibrator.getEstimatedMyz());
26780 assertNull(calibrator.getEstimatedMzx());
26781 assertNull(calibrator.getEstimatedMzy());
26782 assertNull(calibrator.getEstimatedCovariance());
26783 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26784 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26785 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26786 assertNotNull(calibrator.getGroundTruthGravityNorm());
26787 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26788 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26789 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26790 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26791 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26792 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26793 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26794 }
26795
26796 @Test
26797 public void testConstructor208() throws WrongSizeException {
26798 final Matrix ba = generateBa();
26799 final double biasX = ba.getElementAtIndex(0);
26800 final double biasY = ba.getElementAtIndex(1);
26801 final double biasZ = ba.getElementAtIndex(2);
26802
26803 final Acceleration bx = new Acceleration(biasX,
26804 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26805 final Acceleration by = new Acceleration(biasY,
26806 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26807 final Acceleration bz = new Acceleration(biasZ,
26808 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26809
26810 final Matrix ma = generateMaCommonAxis();
26811 final double sx = ma.getElementAt(0, 0);
26812 final double sy = ma.getElementAt(1, 1);
26813 final double sz = ma.getElementAt(2, 2);
26814 final double mxy = ma.getElementAt(0, 1);
26815 final double mxz = ma.getElementAt(0, 2);
26816 final double myx = ma.getElementAt(1, 0);
26817 final double myz = ma.getElementAt(1, 2);
26818 final double mzx = ma.getElementAt(2, 0);
26819 final double mzy = ma.getElementAt(2, 1);
26820
26821 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26822 final double latitude = Math.toRadians(
26823 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26824 final double longitude = Math.toRadians(
26825 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26826 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26827 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26828 final NEDVelocity nedVelocity = new NEDVelocity();
26829 final ECEFPosition ecefPosition = new ECEFPosition();
26830 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26831 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26832 ecefPosition, ecefVelocity);
26833
26834 final KnownPositionAccelerometerCalibrator calibrator =
26835 new KnownPositionAccelerometerCalibrator(nedPosition,
26836 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26837 myx, myz, mzx, mzy, this);
26838
26839
26840 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26841 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26842 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26843 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26844 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26845 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26846 final Acceleration bx2 = new Acceleration(0.0,
26847 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26848 calibrator.getInitialBiasXAsAcceleration(bx2);
26849 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
26850 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26851 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
26852 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
26853 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26854 final Acceleration by2 = new Acceleration(0.0,
26855 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26856 calibrator.getInitialBiasYAsAcceleration(by2);
26857 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
26858 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26859 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
26860 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
26861 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26862 final Acceleration bz2 = new Acceleration(0.0,
26863 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26864 calibrator.getInitialBiasZAsAcceleration(bz2);
26865 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
26866 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26867 assertEquals(calibrator.getInitialSx(), sx, 0.0);
26868 assertEquals(calibrator.getInitialSy(), sy, 0.0);
26869 assertEquals(calibrator.getInitialSz(), sz, 0.0);
26870 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
26871 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
26872 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
26873 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
26874 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
26875 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
26876 final double[] bias1 = calibrator.getInitialBias();
26877 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
26878 final double[] bias2 = new double[3];
26879 calibrator.getInitialBias(bias2);
26880 assertArrayEquals(bias1, bias2, 0.0);
26881 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
26882 assertEquals(b1, ba);
26883 final Matrix b2 = new Matrix(3, 1);
26884 calibrator.getInitialBiasAsMatrix(b2);
26885 assertEquals(b1, b2);
26886 final Matrix ma1 = new Matrix(3, 3);
26887 ma1.setSubmatrix(0, 0,
26888 2, 2,
26889 new double[]{sx, myx, mzx,
26890 mxy, sy, mzy,
26891 mxz, myz, sz});
26892 assertEquals(calibrator.getInitialMa(), ma1);
26893 final Matrix ma2 = new Matrix(3, 3);
26894 calibrator.getInitialMa(ma2);
26895 assertEquals(ma1, ma2);
26896 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
26897 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
26898 final NEDPosition nedPosition1 = new NEDPosition();
26899 assertTrue(calibrator.getNedPosition(nedPosition1));
26900 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
26901 assertNull(calibrator.getMeasurements());
26902 assertTrue(calibrator.isCommonAxisUsed());
26903 assertSame(calibrator.getListener(), this);
26904 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
26905 assertFalse(calibrator.isReady());
26906 assertFalse(calibrator.isRunning());
26907 assertNull(calibrator.getEstimatedBiases());
26908 assertFalse(calibrator.getEstimatedBiases(null));
26909 assertNull(calibrator.getEstimatedBiasesAsMatrix());
26910 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
26911 assertNull(calibrator.getEstimatedBiasFx());
26912 assertNull(calibrator.getEstimatedBiasFy());
26913 assertNull(calibrator.getEstimatedBiasFz());
26914 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
26915 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
26916 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
26917 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
26918 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
26919 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
26920 assertNull(calibrator.getEstimatedMa());
26921 assertNull(calibrator.getEstimatedSx());
26922 assertNull(calibrator.getEstimatedSy());
26923 assertNull(calibrator.getEstimatedSz());
26924 assertNull(calibrator.getEstimatedMxy());
26925 assertNull(calibrator.getEstimatedMxz());
26926 assertNull(calibrator.getEstimatedMyx());
26927 assertNull(calibrator.getEstimatedMyz());
26928 assertNull(calibrator.getEstimatedMzx());
26929 assertNull(calibrator.getEstimatedMzy());
26930 assertNull(calibrator.getEstimatedCovariance());
26931 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
26932 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
26933 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
26934 assertNotNull(calibrator.getGroundTruthGravityNorm());
26935 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
26936 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
26937 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
26938 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
26939 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
26940 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
26941 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
26942 }
26943
26944 @Test
26945 public void testConstructor209() throws WrongSizeException {
26946 final Collection<StandardDeviationBodyKinematics> measurements =
26947 Collections.emptyList();
26948
26949 final Matrix ba = generateBa();
26950 final double biasX = ba.getElementAtIndex(0);
26951 final double biasY = ba.getElementAtIndex(1);
26952 final double biasZ = ba.getElementAtIndex(2);
26953
26954 final Acceleration bx = new Acceleration(biasX,
26955 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26956 final Acceleration by = new Acceleration(biasY,
26957 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26958 final Acceleration bz = new Acceleration(biasZ,
26959 AccelerationUnit.METERS_PER_SQUARED_SECOND);
26960
26961 final Matrix ma = generateMaCommonAxis();
26962 final double sx = ma.getElementAt(0, 0);
26963 final double sy = ma.getElementAt(1, 1);
26964 final double sz = ma.getElementAt(2, 2);
26965 final double mxy = ma.getElementAt(0, 1);
26966 final double mxz = ma.getElementAt(0, 2);
26967 final double myx = ma.getElementAt(1, 0);
26968 final double myz = ma.getElementAt(1, 2);
26969 final double mzx = ma.getElementAt(2, 0);
26970 final double mzy = ma.getElementAt(2, 1);
26971
26972 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
26973 final double latitude = Math.toRadians(
26974 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
26975 final double longitude = Math.toRadians(
26976 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
26977 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
26978 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
26979 final NEDVelocity nedVelocity = new NEDVelocity();
26980 final ECEFPosition ecefPosition = new ECEFPosition();
26981 final ECEFVelocity ecefVelocity = new ECEFVelocity();
26982 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
26983 ecefPosition, ecefVelocity);
26984
26985 final KnownPositionAccelerometerCalibrator calibrator =
26986 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
26987 true, bx, by, bz, sx, sy, sz, mxy, mxz,
26988 myx, myz, mzx, mzy);
26989
26990
26991 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
26992 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
26993 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
26994 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
26995 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
26996 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
26997 final Acceleration bx2 = new Acceleration(0.0,
26998 AccelerationUnit.FEET_PER_SQUARED_SECOND);
26999 calibrator.getInitialBiasXAsAcceleration(bx2);
27000 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27001 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27002 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27003 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27004 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27005 final Acceleration by2 = new Acceleration(0.0,
27006 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27007 calibrator.getInitialBiasYAsAcceleration(by2);
27008 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27009 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27010 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27011 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27012 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27013 final Acceleration bz2 = new Acceleration(0.0,
27014 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27015 calibrator.getInitialBiasZAsAcceleration(bz2);
27016 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27017 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27018 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27019 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27020 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27021 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27022 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27023 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27024 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27025 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27026 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27027 final double[] bias1 = calibrator.getInitialBias();
27028 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27029 final double[] bias2 = new double[3];
27030 calibrator.getInitialBias(bias2);
27031 assertArrayEquals(bias1, bias2, 0.0);
27032 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27033 assertEquals(b1, ba);
27034 final Matrix b2 = new Matrix(3, 1);
27035 calibrator.getInitialBiasAsMatrix(b2);
27036 assertEquals(b1, b2);
27037 final Matrix ma1 = new Matrix(3, 3);
27038 ma1.setSubmatrix(0, 0,
27039 2, 2,
27040 new double[]{sx, myx, mzx,
27041 mxy, sy, mzy,
27042 mxz, myz, sz});
27043 assertEquals(calibrator.getInitialMa(), ma1);
27044 final Matrix ma2 = new Matrix(3, 3);
27045 calibrator.getInitialMa(ma2);
27046 assertEquals(ma1, ma2);
27047 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27048 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27049 final NEDPosition nedPosition1 = new NEDPosition();
27050 assertTrue(calibrator.getNedPosition(nedPosition1));
27051 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27052 assertSame(calibrator.getMeasurements(), measurements);
27053 assertTrue(calibrator.isCommonAxisUsed());
27054 assertNull(calibrator.getListener());
27055 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27056 assertFalse(calibrator.isReady());
27057 assertFalse(calibrator.isRunning());
27058 assertNull(calibrator.getEstimatedBiases());
27059 assertFalse(calibrator.getEstimatedBiases(null));
27060 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27061 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27062 assertNull(calibrator.getEstimatedBiasFx());
27063 assertNull(calibrator.getEstimatedBiasFy());
27064 assertNull(calibrator.getEstimatedBiasFz());
27065 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27066 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27067 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27068 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27069 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27070 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27071 assertNull(calibrator.getEstimatedMa());
27072 assertNull(calibrator.getEstimatedSx());
27073 assertNull(calibrator.getEstimatedSy());
27074 assertNull(calibrator.getEstimatedSz());
27075 assertNull(calibrator.getEstimatedMxy());
27076 assertNull(calibrator.getEstimatedMxz());
27077 assertNull(calibrator.getEstimatedMyx());
27078 assertNull(calibrator.getEstimatedMyz());
27079 assertNull(calibrator.getEstimatedMzx());
27080 assertNull(calibrator.getEstimatedMzy());
27081 assertNull(calibrator.getEstimatedCovariance());
27082 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27083 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27084 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27085 assertNotNull(calibrator.getGroundTruthGravityNorm());
27086 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27087 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27088 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27089 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27090 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27091 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27092 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27093 }
27094
27095 @Test
27096 public void testConstructor210() throws WrongSizeException {
27097 final Collection<StandardDeviationBodyKinematics> measurements =
27098 Collections.emptyList();
27099
27100 final Matrix ba = generateBa();
27101 final double biasX = ba.getElementAtIndex(0);
27102 final double biasY = ba.getElementAtIndex(1);
27103 final double biasZ = ba.getElementAtIndex(2);
27104
27105 final Acceleration bx = new Acceleration(biasX,
27106 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27107 final Acceleration by = new Acceleration(biasY,
27108 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27109 final Acceleration bz = new Acceleration(biasZ,
27110 AccelerationUnit.METERS_PER_SQUARED_SECOND);
27111
27112 final Matrix ma = generateMaCommonAxis();
27113 final double sx = ma.getElementAt(0, 0);
27114 final double sy = ma.getElementAt(1, 1);
27115 final double sz = ma.getElementAt(2, 2);
27116 final double mxy = ma.getElementAt(0, 1);
27117 final double mxz = ma.getElementAt(0, 2);
27118 final double myx = ma.getElementAt(1, 0);
27119 final double myz = ma.getElementAt(1, 2);
27120 final double mzx = ma.getElementAt(2, 0);
27121 final double mzy = ma.getElementAt(2, 1);
27122
27123 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27124 final double latitude = Math.toRadians(
27125 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27126 final double longitude = Math.toRadians(
27127 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27128 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27129 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27130 final NEDVelocity nedVelocity = new NEDVelocity();
27131 final ECEFPosition ecefPosition = new ECEFPosition();
27132 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27133 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27134 ecefPosition, ecefVelocity);
27135
27136 final KnownPositionAccelerometerCalibrator calibrator =
27137 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
27138 true, bx, by, bz, sx, sy, sz, mxy, mxz,
27139 myx, myz, mzx, mzy, this);
27140
27141
27142 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27143 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27144 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27145 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27146 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27147 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27148 final Acceleration bx2 = new Acceleration(0.0,
27149 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27150 calibrator.getInitialBiasXAsAcceleration(bx2);
27151 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27152 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27153 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27154 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27155 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27156 final Acceleration by2 = new Acceleration(0.0,
27157 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27158 calibrator.getInitialBiasYAsAcceleration(by2);
27159 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27160 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27161 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27162 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27163 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27164 final Acceleration bz2 = new Acceleration(0.0,
27165 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27166 calibrator.getInitialBiasZAsAcceleration(bz2);
27167 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27168 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27169 assertEquals(calibrator.getInitialSx(), sx, 0.0);
27170 assertEquals(calibrator.getInitialSy(), sy, 0.0);
27171 assertEquals(calibrator.getInitialSz(), sz, 0.0);
27172 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
27173 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
27174 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
27175 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
27176 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
27177 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
27178 final double[] bias1 = calibrator.getInitialBias();
27179 assertArrayEquals(bias1, new double[]{biasX, biasY, biasZ}, 0.0);
27180 final double[] bias2 = new double[3];
27181 calibrator.getInitialBias(bias2);
27182 assertArrayEquals(bias1, bias2, 0.0);
27183 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27184 assertEquals(b1, ba);
27185 final Matrix b2 = new Matrix(3, 1);
27186 calibrator.getInitialBiasAsMatrix(b2);
27187 assertEquals(b1, b2);
27188 final Matrix ma1 = new Matrix(3, 3);
27189 ma1.setSubmatrix(0, 0,
27190 2, 2,
27191 new double[]{sx, myx, mzx,
27192 mxy, sy, mzy,
27193 mxz, myz, sz});
27194 assertEquals(calibrator.getInitialMa(), ma1);
27195 final Matrix ma2 = new Matrix(3, 3);
27196 calibrator.getInitialMa(ma2);
27197 assertEquals(ma1, ma2);
27198 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27199 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27200 final NEDPosition nedPosition1 = new NEDPosition();
27201 assertTrue(calibrator.getNedPosition(nedPosition1));
27202 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27203 assertSame(calibrator.getMeasurements(), measurements);
27204 assertTrue(calibrator.isCommonAxisUsed());
27205 assertSame(calibrator.getListener(), this);
27206 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27207 assertFalse(calibrator.isReady());
27208 assertFalse(calibrator.isRunning());
27209 assertNull(calibrator.getEstimatedBiases());
27210 assertFalse(calibrator.getEstimatedBiases(null));
27211 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27212 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27213 assertNull(calibrator.getEstimatedBiasFx());
27214 assertNull(calibrator.getEstimatedBiasFy());
27215 assertNull(calibrator.getEstimatedBiasFz());
27216 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27217 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27218 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27219 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27220 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27221 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27222 assertNull(calibrator.getEstimatedMa());
27223 assertNull(calibrator.getEstimatedSx());
27224 assertNull(calibrator.getEstimatedSy());
27225 assertNull(calibrator.getEstimatedSz());
27226 assertNull(calibrator.getEstimatedMxy());
27227 assertNull(calibrator.getEstimatedMxz());
27228 assertNull(calibrator.getEstimatedMyx());
27229 assertNull(calibrator.getEstimatedMyz());
27230 assertNull(calibrator.getEstimatedMzx());
27231 assertNull(calibrator.getEstimatedMzy());
27232 assertNull(calibrator.getEstimatedCovariance());
27233 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27234 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27235 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27236 assertNotNull(calibrator.getGroundTruthGravityNorm());
27237 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27238 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27239 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27240 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27241 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27242 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27243 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27244 }
27245
27246 @Test
27247 public void testConstructor211() throws WrongSizeException {
27248 final Matrix ba = generateBa();
27249 final double[] bias = ba.getBuffer();
27250 final double biasX = ba.getElementAtIndex(0);
27251 final double biasY = ba.getElementAtIndex(1);
27252 final double biasZ = ba.getElementAtIndex(2);
27253
27254 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27255 final double latitude = Math.toRadians(
27256 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27257 final double longitude = Math.toRadians(
27258 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27259 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27260 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27261 final NEDVelocity nedVelocity = new NEDVelocity();
27262 final ECEFPosition ecefPosition = new ECEFPosition();
27263 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27264 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27265 ecefPosition, ecefVelocity);
27266
27267 KnownPositionAccelerometerCalibrator calibrator =
27268 new KnownPositionAccelerometerCalibrator(nedPosition, bias);
27269
27270
27271 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27272 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27273 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27274 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27275 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27276 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27277 final Acceleration bx2 = new Acceleration(0.0,
27278 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27279 calibrator.getInitialBiasXAsAcceleration(bx2);
27280 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27281 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27282 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27283 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27284 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27285 final Acceleration by2 = new Acceleration(0.0,
27286 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27287 calibrator.getInitialBiasYAsAcceleration(by2);
27288 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27289 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27290 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27291 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27292 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27293 final Acceleration bz2 = new Acceleration(0.0,
27294 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27295 calibrator.getInitialBiasZAsAcceleration(bz2);
27296 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27297 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27298 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27299 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27300 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27301 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27302 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27303 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27304 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27305 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27306 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27307 final double[] bias1 = calibrator.getInitialBias();
27308 assertArrayEquals(bias1, bias, 0.0);
27309 final double[] bias2 = new double[3];
27310 calibrator.getInitialBias(bias2);
27311 assertArrayEquals(bias1, bias2, 0.0);
27312 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27313 assertEquals(b1, ba);
27314 final Matrix b2 = new Matrix(3, 1);
27315 calibrator.getInitialBiasAsMatrix(b2);
27316 assertEquals(b1, b2);
27317 final Matrix ma1 = calibrator.getInitialMa();
27318 assertEquals(ma1, new Matrix(3, 3));
27319 final Matrix ma2 = new Matrix(3, 3);
27320 calibrator.getInitialMa(ma2);
27321 assertEquals(ma1, ma2);
27322 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27323 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27324 final NEDPosition nedPosition1 = new NEDPosition();
27325 assertTrue(calibrator.getNedPosition(nedPosition1));
27326 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27327 assertNull(calibrator.getMeasurements());
27328 assertFalse(calibrator.isCommonAxisUsed());
27329 assertNull(calibrator.getListener());
27330 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27331 assertFalse(calibrator.isReady());
27332 assertFalse(calibrator.isRunning());
27333 assertNull(calibrator.getEstimatedBiases());
27334 assertFalse(calibrator.getEstimatedBiases(null));
27335 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27336 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27337 assertNull(calibrator.getEstimatedBiasFx());
27338 assertNull(calibrator.getEstimatedBiasFy());
27339 assertNull(calibrator.getEstimatedBiasFz());
27340 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27341 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27342 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27343 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27344 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27345 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27346 assertNull(calibrator.getEstimatedMa());
27347 assertNull(calibrator.getEstimatedSx());
27348 assertNull(calibrator.getEstimatedSy());
27349 assertNull(calibrator.getEstimatedSz());
27350 assertNull(calibrator.getEstimatedMxy());
27351 assertNull(calibrator.getEstimatedMxz());
27352 assertNull(calibrator.getEstimatedMyx());
27353 assertNull(calibrator.getEstimatedMyz());
27354 assertNull(calibrator.getEstimatedMzx());
27355 assertNull(calibrator.getEstimatedMzy());
27356 assertNull(calibrator.getEstimatedCovariance());
27357 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27358 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27359 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27360 assertNotNull(calibrator.getGroundTruthGravityNorm());
27361 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27362 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27363 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27364 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27365 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27366 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27367 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27368
27369
27370 calibrator = null;
27371 try {
27372 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27373 new double[1]);
27374 fail("IllegalArgumentException expected but not thrown");
27375 } catch (final IllegalArgumentException ignore) {
27376 }
27377 assertNull(calibrator);
27378 }
27379
27380 @Test
27381 public void testConstructor212() throws WrongSizeException {
27382 final Matrix ba = generateBa();
27383 final double[] bias = ba.getBuffer();
27384 final double biasX = ba.getElementAtIndex(0);
27385 final double biasY = ba.getElementAtIndex(1);
27386 final double biasZ = ba.getElementAtIndex(2);
27387
27388 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27389 final double latitude = Math.toRadians(
27390 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27391 final double longitude = Math.toRadians(
27392 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27393 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27394 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27395 final NEDVelocity nedVelocity = new NEDVelocity();
27396 final ECEFPosition ecefPosition = new ECEFPosition();
27397 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27398 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27399 ecefPosition, ecefVelocity);
27400
27401 KnownPositionAccelerometerCalibrator calibrator =
27402 new KnownPositionAccelerometerCalibrator(nedPosition, bias,
27403 this);
27404
27405
27406 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27407 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27408 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27409 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27410 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27411 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27412 final Acceleration bx2 = new Acceleration(0.0,
27413 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27414 calibrator.getInitialBiasXAsAcceleration(bx2);
27415 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27416 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27417 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27418 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27419 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27420 final Acceleration by2 = new Acceleration(0.0,
27421 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27422 calibrator.getInitialBiasYAsAcceleration(by2);
27423 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27424 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27425 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27426 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27427 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27428 final Acceleration bz2 = new Acceleration(0.0,
27429 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27430 calibrator.getInitialBiasZAsAcceleration(bz2);
27431 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27432 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27433 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27434 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27435 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27436 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27437 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27438 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27439 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27440 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27441 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27442 final double[] bias1 = calibrator.getInitialBias();
27443 assertArrayEquals(bias1, bias, 0.0);
27444 final double[] bias2 = new double[3];
27445 calibrator.getInitialBias(bias2);
27446 assertArrayEquals(bias1, bias2, 0.0);
27447 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27448 assertEquals(b1, ba);
27449 final Matrix b2 = new Matrix(3, 1);
27450 calibrator.getInitialBiasAsMatrix(b2);
27451 assertEquals(b1, b2);
27452 final Matrix ma1 = calibrator.getInitialMa();
27453 assertEquals(ma1, new Matrix(3, 3));
27454 final Matrix ma2 = new Matrix(3, 3);
27455 calibrator.getInitialMa(ma2);
27456 assertEquals(ma1, ma2);
27457 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27458 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27459 final NEDPosition nedPosition1 = new NEDPosition();
27460 assertTrue(calibrator.getNedPosition(nedPosition1));
27461 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27462 assertNull(calibrator.getMeasurements());
27463 assertFalse(calibrator.isCommonAxisUsed());
27464 assertSame(calibrator.getListener(), this);
27465 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27466 assertFalse(calibrator.isReady());
27467 assertFalse(calibrator.isRunning());
27468 assertNull(calibrator.getEstimatedBiases());
27469 assertFalse(calibrator.getEstimatedBiases(null));
27470 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27471 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27472 assertNull(calibrator.getEstimatedBiasFx());
27473 assertNull(calibrator.getEstimatedBiasFy());
27474 assertNull(calibrator.getEstimatedBiasFz());
27475 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27476 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27477 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27478 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27479 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27480 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27481 assertNull(calibrator.getEstimatedMa());
27482 assertNull(calibrator.getEstimatedSx());
27483 assertNull(calibrator.getEstimatedSy());
27484 assertNull(calibrator.getEstimatedSz());
27485 assertNull(calibrator.getEstimatedMxy());
27486 assertNull(calibrator.getEstimatedMxz());
27487 assertNull(calibrator.getEstimatedMyx());
27488 assertNull(calibrator.getEstimatedMyz());
27489 assertNull(calibrator.getEstimatedMzx());
27490 assertNull(calibrator.getEstimatedMzy());
27491 assertNull(calibrator.getEstimatedCovariance());
27492 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27493 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27494 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27495 assertNotNull(calibrator.getGroundTruthGravityNorm());
27496 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27497 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27498 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27499 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27500 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27501 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27502 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27503
27504
27505 calibrator = null;
27506 try {
27507 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27508 new double[1], this);
27509 fail("IllegalArgumentException expected but not thrown");
27510 } catch (final IllegalArgumentException ignore) {
27511 }
27512 assertNull(calibrator);
27513 }
27514
27515 @Test
27516 public void testConstructor213() throws WrongSizeException {
27517 final Collection<StandardDeviationBodyKinematics> measurements =
27518 Collections.emptyList();
27519
27520 final Matrix ba = generateBa();
27521 final double[] bias = ba.getBuffer();
27522 final double biasX = ba.getElementAtIndex(0);
27523 final double biasY = ba.getElementAtIndex(1);
27524 final double biasZ = ba.getElementAtIndex(2);
27525
27526 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27527 final double latitude = Math.toRadians(
27528 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27529 final double longitude = Math.toRadians(
27530 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27531 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27532 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27533 final NEDVelocity nedVelocity = new NEDVelocity();
27534 final ECEFPosition ecefPosition = new ECEFPosition();
27535 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27536 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27537 ecefPosition, ecefVelocity);
27538
27539 KnownPositionAccelerometerCalibrator calibrator =
27540 new KnownPositionAccelerometerCalibrator(nedPosition,
27541 measurements, bias);
27542
27543
27544 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27545 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27546 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27547 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27548 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27549 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27550 final Acceleration bx2 = new Acceleration(0.0,
27551 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27552 calibrator.getInitialBiasXAsAcceleration(bx2);
27553 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27554 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27555 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27556 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27557 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27558 final Acceleration by2 = new Acceleration(0.0,
27559 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27560 calibrator.getInitialBiasYAsAcceleration(by2);
27561 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27562 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27563 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27564 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27565 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27566 final Acceleration bz2 = new Acceleration(0.0,
27567 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27568 calibrator.getInitialBiasZAsAcceleration(bz2);
27569 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27570 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27571 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27572 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27573 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27574 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27575 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27576 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27577 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27578 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27579 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27580 final double[] bias1 = calibrator.getInitialBias();
27581 assertArrayEquals(bias1, bias, 0.0);
27582 final double[] bias2 = new double[3];
27583 calibrator.getInitialBias(bias2);
27584 assertArrayEquals(bias1, bias2, 0.0);
27585 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27586 assertEquals(b1, ba);
27587 final Matrix b2 = new Matrix(3, 1);
27588 calibrator.getInitialBiasAsMatrix(b2);
27589 assertEquals(b1, b2);
27590 final Matrix ma1 = calibrator.getInitialMa();
27591 assertEquals(ma1, new Matrix(3, 3));
27592 final Matrix ma2 = new Matrix(3, 3);
27593 calibrator.getInitialMa(ma2);
27594 assertEquals(ma1, ma2);
27595 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27596 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27597 final NEDPosition nedPosition1 = new NEDPosition();
27598 assertTrue(calibrator.getNedPosition(nedPosition1));
27599 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27600 assertSame(calibrator.getMeasurements(), measurements);
27601 assertFalse(calibrator.isCommonAxisUsed());
27602 assertNull(calibrator.getListener());
27603 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27604 assertFalse(calibrator.isReady());
27605 assertFalse(calibrator.isRunning());
27606 assertNull(calibrator.getEstimatedBiases());
27607 assertFalse(calibrator.getEstimatedBiases(null));
27608 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27609 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27610 assertNull(calibrator.getEstimatedBiasFx());
27611 assertNull(calibrator.getEstimatedBiasFy());
27612 assertNull(calibrator.getEstimatedBiasFz());
27613 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27614 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27615 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27616 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27617 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27618 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27619 assertNull(calibrator.getEstimatedMa());
27620 assertNull(calibrator.getEstimatedSx());
27621 assertNull(calibrator.getEstimatedSy());
27622 assertNull(calibrator.getEstimatedSz());
27623 assertNull(calibrator.getEstimatedMxy());
27624 assertNull(calibrator.getEstimatedMxz());
27625 assertNull(calibrator.getEstimatedMyx());
27626 assertNull(calibrator.getEstimatedMyz());
27627 assertNull(calibrator.getEstimatedMzx());
27628 assertNull(calibrator.getEstimatedMzy());
27629 assertNull(calibrator.getEstimatedCovariance());
27630 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27631 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27632 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27633 assertNotNull(calibrator.getGroundTruthGravityNorm());
27634 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27635 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27636 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27637 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27638 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27639 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27640 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27641
27642
27643 calibrator = null;
27644 try {
27645 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27646 measurements, new double[1]);
27647 fail("IllegalArgumentException expected but not thrown");
27648 } catch (final IllegalArgumentException ignore) {
27649 }
27650 assertNull(calibrator);
27651 }
27652
27653 @Test
27654 public void testConstructor214() throws WrongSizeException {
27655 final Collection<StandardDeviationBodyKinematics> measurements =
27656 Collections.emptyList();
27657
27658 final Matrix ba = generateBa();
27659 final double[] bias = ba.getBuffer();
27660 final double biasX = ba.getElementAtIndex(0);
27661 final double biasY = ba.getElementAtIndex(1);
27662 final double biasZ = ba.getElementAtIndex(2);
27663
27664 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27665 final double latitude = Math.toRadians(
27666 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27667 final double longitude = Math.toRadians(
27668 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27669 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27670 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27671 final NEDVelocity nedVelocity = new NEDVelocity();
27672 final ECEFPosition ecefPosition = new ECEFPosition();
27673 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27674 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27675 ecefPosition, ecefVelocity);
27676
27677 KnownPositionAccelerometerCalibrator calibrator =
27678 new KnownPositionAccelerometerCalibrator(nedPosition,
27679 measurements, bias, this);
27680
27681
27682 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27683 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27684 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27685 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27686 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27687 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27688 final Acceleration bx2 = new Acceleration(0.0,
27689 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27690 calibrator.getInitialBiasXAsAcceleration(bx2);
27691 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27692 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27693 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27694 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27695 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27696 final Acceleration by2 = new Acceleration(0.0,
27697 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27698 calibrator.getInitialBiasYAsAcceleration(by2);
27699 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27700 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27701 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27702 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27703 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27704 final Acceleration bz2 = new Acceleration(0.0,
27705 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27706 calibrator.getInitialBiasZAsAcceleration(bz2);
27707 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27708 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27709 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27710 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27711 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27712 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27713 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27714 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27715 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27716 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27717 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27718 final double[] bias1 = calibrator.getInitialBias();
27719 assertArrayEquals(bias1, bias, 0.0);
27720 final double[] bias2 = new double[3];
27721 calibrator.getInitialBias(bias2);
27722 assertArrayEquals(bias1, bias2, 0.0);
27723 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27724 assertEquals(b1, ba);
27725 final Matrix b2 = new Matrix(3, 1);
27726 calibrator.getInitialBiasAsMatrix(b2);
27727 assertEquals(b1, b2);
27728 final Matrix ma1 = calibrator.getInitialMa();
27729 assertEquals(ma1, new Matrix(3, 3));
27730 final Matrix ma2 = new Matrix(3, 3);
27731 calibrator.getInitialMa(ma2);
27732 assertEquals(ma1, ma2);
27733 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27734 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27735 final NEDPosition nedPosition1 = new NEDPosition();
27736 assertTrue(calibrator.getNedPosition(nedPosition1));
27737 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27738 assertSame(calibrator.getMeasurements(), measurements);
27739 assertFalse(calibrator.isCommonAxisUsed());
27740 assertSame(calibrator.getListener(), this);
27741 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
27742 assertFalse(calibrator.isReady());
27743 assertFalse(calibrator.isRunning());
27744 assertNull(calibrator.getEstimatedBiases());
27745 assertFalse(calibrator.getEstimatedBiases(null));
27746 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27747 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27748 assertNull(calibrator.getEstimatedBiasFx());
27749 assertNull(calibrator.getEstimatedBiasFy());
27750 assertNull(calibrator.getEstimatedBiasFz());
27751 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27752 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27753 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27754 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27755 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27756 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27757 assertNull(calibrator.getEstimatedMa());
27758 assertNull(calibrator.getEstimatedSx());
27759 assertNull(calibrator.getEstimatedSy());
27760 assertNull(calibrator.getEstimatedSz());
27761 assertNull(calibrator.getEstimatedMxy());
27762 assertNull(calibrator.getEstimatedMxz());
27763 assertNull(calibrator.getEstimatedMyx());
27764 assertNull(calibrator.getEstimatedMyz());
27765 assertNull(calibrator.getEstimatedMzx());
27766 assertNull(calibrator.getEstimatedMzy());
27767 assertNull(calibrator.getEstimatedCovariance());
27768 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27769 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27770 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27771 assertNotNull(calibrator.getGroundTruthGravityNorm());
27772 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27773 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27774 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27775 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27776 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27777 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27778 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27779
27780
27781 calibrator = null;
27782 try {
27783 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27784 measurements, new double[1], this);
27785 fail("IllegalArgumentException expected but not thrown");
27786 } catch (final IllegalArgumentException ignore) {
27787 }
27788 assertNull(calibrator);
27789 }
27790
27791 @Test
27792 public void testConstructor215() throws WrongSizeException {
27793 final Matrix ba = generateBa();
27794 final double[] bias = ba.getBuffer();
27795 final double biasX = ba.getElementAtIndex(0);
27796 final double biasY = ba.getElementAtIndex(1);
27797 final double biasZ = ba.getElementAtIndex(2);
27798
27799 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27800 final double latitude = Math.toRadians(
27801 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27802 final double longitude = Math.toRadians(
27803 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27804 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27805 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27806 final NEDVelocity nedVelocity = new NEDVelocity();
27807 final ECEFPosition ecefPosition = new ECEFPosition();
27808 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27809 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27810 ecefPosition, ecefVelocity);
27811
27812 KnownPositionAccelerometerCalibrator calibrator =
27813 new KnownPositionAccelerometerCalibrator(nedPosition,
27814 true, bias);
27815
27816
27817 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27818 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27819 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27820 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27821 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27822 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27823 final Acceleration bx2 = new Acceleration(0.0,
27824 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27825 calibrator.getInitialBiasXAsAcceleration(bx2);
27826 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27827 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27828 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27829 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27830 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27831 final Acceleration by2 = new Acceleration(0.0,
27832 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27833 calibrator.getInitialBiasYAsAcceleration(by2);
27834 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27835 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27836 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27837 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27838 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27839 final Acceleration bz2 = new Acceleration(0.0,
27840 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27841 calibrator.getInitialBiasZAsAcceleration(bz2);
27842 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27843 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27844 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27845 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27846 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27847 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27848 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27849 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27850 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27851 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27852 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27853 final double[] bias1 = calibrator.getInitialBias();
27854 assertArrayEquals(bias1, bias, 0.0);
27855 final double[] bias2 = new double[3];
27856 calibrator.getInitialBias(bias2);
27857 assertArrayEquals(bias1, bias2, 0.0);
27858 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27859 assertEquals(b1, ba);
27860 final Matrix b2 = new Matrix(3, 1);
27861 calibrator.getInitialBiasAsMatrix(b2);
27862 assertEquals(b1, b2);
27863 final Matrix ma1 = calibrator.getInitialMa();
27864 assertEquals(ma1, new Matrix(3, 3));
27865 final Matrix ma2 = new Matrix(3, 3);
27866 calibrator.getInitialMa(ma2);
27867 assertEquals(ma1, ma2);
27868 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
27869 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
27870 final NEDPosition nedPosition1 = new NEDPosition();
27871 assertTrue(calibrator.getNedPosition(nedPosition1));
27872 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
27873 assertNull(calibrator.getMeasurements());
27874 assertTrue(calibrator.isCommonAxisUsed());
27875 assertNull(calibrator.getListener());
27876 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
27877 assertFalse(calibrator.isReady());
27878 assertFalse(calibrator.isRunning());
27879 assertNull(calibrator.getEstimatedBiases());
27880 assertFalse(calibrator.getEstimatedBiases(null));
27881 assertNull(calibrator.getEstimatedBiasesAsMatrix());
27882 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
27883 assertNull(calibrator.getEstimatedBiasFx());
27884 assertNull(calibrator.getEstimatedBiasFy());
27885 assertNull(calibrator.getEstimatedBiasFz());
27886 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
27887 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
27888 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
27889 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
27890 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
27891 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
27892 assertNull(calibrator.getEstimatedMa());
27893 assertNull(calibrator.getEstimatedSx());
27894 assertNull(calibrator.getEstimatedSy());
27895 assertNull(calibrator.getEstimatedSz());
27896 assertNull(calibrator.getEstimatedMxy());
27897 assertNull(calibrator.getEstimatedMxz());
27898 assertNull(calibrator.getEstimatedMyx());
27899 assertNull(calibrator.getEstimatedMyz());
27900 assertNull(calibrator.getEstimatedMzx());
27901 assertNull(calibrator.getEstimatedMzy());
27902 assertNull(calibrator.getEstimatedCovariance());
27903 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
27904 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
27905 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
27906 assertNotNull(calibrator.getGroundTruthGravityNorm());
27907 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
27908 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
27909 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
27910 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
27911 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
27912 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
27913 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
27914
27915
27916 calibrator = null;
27917 try {
27918 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
27919 true, new double[1]);
27920 fail("IllegalArgumentException expected but not thrown");
27921 } catch (final IllegalArgumentException ignore) {
27922 }
27923 assertNull(calibrator);
27924 }
27925
27926 @Test
27927 public void testConstructor216() throws WrongSizeException {
27928 final Matrix ba = generateBa();
27929 final double[] bias = ba.getBuffer();
27930 final double biasX = ba.getElementAtIndex(0);
27931 final double biasY = ba.getElementAtIndex(1);
27932 final double biasZ = ba.getElementAtIndex(2);
27933
27934 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
27935 final double latitude = Math.toRadians(
27936 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
27937 final double longitude = Math.toRadians(
27938 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
27939 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
27940 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
27941 final NEDVelocity nedVelocity = new NEDVelocity();
27942 final ECEFPosition ecefPosition = new ECEFPosition();
27943 final ECEFVelocity ecefVelocity = new ECEFVelocity();
27944 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
27945 ecefPosition, ecefVelocity);
27946
27947 KnownPositionAccelerometerCalibrator calibrator =
27948 new KnownPositionAccelerometerCalibrator(nedPosition,
27949 true, bias, this);
27950
27951
27952 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
27953 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
27954 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
27955 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
27956 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
27957 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27958 final Acceleration bx2 = new Acceleration(0.0,
27959 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27960 calibrator.getInitialBiasXAsAcceleration(bx2);
27961 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
27962 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27963 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
27964 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
27965 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27966 final Acceleration by2 = new Acceleration(0.0,
27967 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27968 calibrator.getInitialBiasYAsAcceleration(by2);
27969 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
27970 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27971 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
27972 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
27973 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27974 final Acceleration bz2 = new Acceleration(0.0,
27975 AccelerationUnit.FEET_PER_SQUARED_SECOND);
27976 calibrator.getInitialBiasZAsAcceleration(bz2);
27977 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
27978 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
27979 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
27980 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
27981 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
27982 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
27983 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
27984 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
27985 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
27986 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
27987 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
27988 final double[] bias1 = calibrator.getInitialBias();
27989 assertArrayEquals(bias1, bias, 0.0);
27990 final double[] bias2 = new double[3];
27991 calibrator.getInitialBias(bias2);
27992 assertArrayEquals(bias1, bias2, 0.0);
27993 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
27994 assertEquals(b1, ba);
27995 final Matrix b2 = new Matrix(3, 1);
27996 calibrator.getInitialBiasAsMatrix(b2);
27997 assertEquals(b1, b2);
27998 final Matrix ma1 = calibrator.getInitialMa();
27999 assertEquals(ma1, new Matrix(3, 3));
28000 final Matrix ma2 = new Matrix(3, 3);
28001 calibrator.getInitialMa(ma2);
28002 assertEquals(ma1, ma2);
28003 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28004 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28005 final NEDPosition nedPosition1 = new NEDPosition();
28006 assertTrue(calibrator.getNedPosition(nedPosition1));
28007 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28008 assertNull(calibrator.getMeasurements());
28009 assertTrue(calibrator.isCommonAxisUsed());
28010 assertSame(calibrator.getListener(), this);
28011 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28012 assertFalse(calibrator.isReady());
28013 assertFalse(calibrator.isRunning());
28014 assertNull(calibrator.getEstimatedBiases());
28015 assertFalse(calibrator.getEstimatedBiases(null));
28016 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28017 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28018 assertNull(calibrator.getEstimatedBiasFx());
28019 assertNull(calibrator.getEstimatedBiasFy());
28020 assertNull(calibrator.getEstimatedBiasFz());
28021 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28022 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28023 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28024 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28025 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28026 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28027 assertNull(calibrator.getEstimatedMa());
28028 assertNull(calibrator.getEstimatedSx());
28029 assertNull(calibrator.getEstimatedSy());
28030 assertNull(calibrator.getEstimatedSz());
28031 assertNull(calibrator.getEstimatedMxy());
28032 assertNull(calibrator.getEstimatedMxz());
28033 assertNull(calibrator.getEstimatedMyx());
28034 assertNull(calibrator.getEstimatedMyz());
28035 assertNull(calibrator.getEstimatedMzx());
28036 assertNull(calibrator.getEstimatedMzy());
28037 assertNull(calibrator.getEstimatedCovariance());
28038 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28039 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28040 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28041 assertNotNull(calibrator.getGroundTruthGravityNorm());
28042 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28043 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28044 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28045 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28046 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28047 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28048 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28049
28050
28051 calibrator = null;
28052 try {
28053 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28054 true, new double[1], this);
28055 fail("IllegalArgumentException expected but not thrown");
28056 } catch (final IllegalArgumentException ignore) {
28057 }
28058 assertNull(calibrator);
28059 }
28060
28061 @Test
28062 public void testConstructor217() throws WrongSizeException {
28063 final Collection<StandardDeviationBodyKinematics> measurements =
28064 Collections.emptyList();
28065
28066 final Matrix ba = generateBa();
28067 final double[] bias = ba.getBuffer();
28068 final double biasX = ba.getElementAtIndex(0);
28069 final double biasY = ba.getElementAtIndex(1);
28070 final double biasZ = ba.getElementAtIndex(2);
28071
28072 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28073 final double latitude = Math.toRadians(
28074 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28075 final double longitude = Math.toRadians(
28076 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28077 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28078 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28079 final NEDVelocity nedVelocity = new NEDVelocity();
28080 final ECEFPosition ecefPosition = new ECEFPosition();
28081 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28082 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28083 ecefPosition, ecefVelocity);
28084
28085 KnownPositionAccelerometerCalibrator calibrator =
28086 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28087 true, bias);
28088
28089
28090 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28091 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28092 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28093 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28094 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28095 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28096 final Acceleration bx2 = new Acceleration(0.0,
28097 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28098 calibrator.getInitialBiasXAsAcceleration(bx2);
28099 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28100 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28101 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28102 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28103 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28104 final Acceleration by2 = new Acceleration(0.0,
28105 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28106 calibrator.getInitialBiasYAsAcceleration(by2);
28107 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28108 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28109 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28110 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28111 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28112 final Acceleration bz2 = new Acceleration(0.0,
28113 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28114 calibrator.getInitialBiasZAsAcceleration(bz2);
28115 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28116 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28117 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28118 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28119 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28120 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28121 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28122 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28123 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28124 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28125 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28126 final double[] bias1 = calibrator.getInitialBias();
28127 assertArrayEquals(bias1, bias, 0.0);
28128 final double[] bias2 = new double[3];
28129 calibrator.getInitialBias(bias2);
28130 assertArrayEquals(bias1, bias2, 0.0);
28131 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28132 assertEquals(b1, ba);
28133 final Matrix b2 = new Matrix(3, 1);
28134 calibrator.getInitialBiasAsMatrix(b2);
28135 assertEquals(b1, b2);
28136 final Matrix ma1 = calibrator.getInitialMa();
28137 assertEquals(ma1, new Matrix(3, 3));
28138 final Matrix ma2 = new Matrix(3, 3);
28139 calibrator.getInitialMa(ma2);
28140 assertEquals(ma1, ma2);
28141 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28142 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28143 final NEDPosition nedPosition1 = new NEDPosition();
28144 assertTrue(calibrator.getNedPosition(nedPosition1));
28145 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28146 assertSame(calibrator.getMeasurements(), measurements);
28147 assertTrue(calibrator.isCommonAxisUsed());
28148 assertNull(calibrator.getListener());
28149 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28150 assertFalse(calibrator.isReady());
28151 assertFalse(calibrator.isRunning());
28152 assertNull(calibrator.getEstimatedBiases());
28153 assertFalse(calibrator.getEstimatedBiases(null));
28154 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28155 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28156 assertNull(calibrator.getEstimatedBiasFx());
28157 assertNull(calibrator.getEstimatedBiasFy());
28158 assertNull(calibrator.getEstimatedBiasFz());
28159 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28160 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28161 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28162 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28163 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28164 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28165 assertNull(calibrator.getEstimatedMa());
28166 assertNull(calibrator.getEstimatedSx());
28167 assertNull(calibrator.getEstimatedSy());
28168 assertNull(calibrator.getEstimatedSz());
28169 assertNull(calibrator.getEstimatedMxy());
28170 assertNull(calibrator.getEstimatedMxz());
28171 assertNull(calibrator.getEstimatedMyx());
28172 assertNull(calibrator.getEstimatedMyz());
28173 assertNull(calibrator.getEstimatedMzx());
28174 assertNull(calibrator.getEstimatedMzy());
28175 assertNull(calibrator.getEstimatedCovariance());
28176 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28177 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28178 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28179 assertNotNull(calibrator.getGroundTruthGravityNorm());
28180 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28181 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28182 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28183 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28184 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28185 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28186 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28187
28188
28189 calibrator = null;
28190 try {
28191 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28192 measurements, true, new double[1]);
28193 fail("IllegalArgumentException expected but not thrown");
28194 } catch (final IllegalArgumentException ignore) {
28195 }
28196 assertNull(calibrator);
28197 }
28198
28199 @Test
28200 public void testConstructor218() throws WrongSizeException {
28201 final Collection<StandardDeviationBodyKinematics> measurements =
28202 Collections.emptyList();
28203
28204 final Matrix ba = generateBa();
28205 final double[] bias = ba.getBuffer();
28206 final double biasX = ba.getElementAtIndex(0);
28207 final double biasY = ba.getElementAtIndex(1);
28208 final double biasZ = ba.getElementAtIndex(2);
28209
28210 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28211 final double latitude = Math.toRadians(
28212 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28213 final double longitude = Math.toRadians(
28214 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28215 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28216 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28217 final NEDVelocity nedVelocity = new NEDVelocity();
28218 final ECEFPosition ecefPosition = new ECEFPosition();
28219 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28220 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28221 ecefPosition, ecefVelocity);
28222
28223 KnownPositionAccelerometerCalibrator calibrator =
28224 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28225 true, bias, this);
28226
28227
28228 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28229 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28230 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28231 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28232 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28233 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28234 final Acceleration bx2 = new Acceleration(0.0,
28235 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28236 calibrator.getInitialBiasXAsAcceleration(bx2);
28237 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28238 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28239 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28240 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28241 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28242 final Acceleration by2 = new Acceleration(0.0,
28243 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28244 calibrator.getInitialBiasYAsAcceleration(by2);
28245 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28246 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28247 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28248 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28249 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28250 final Acceleration bz2 = new Acceleration(0.0,
28251 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28252 calibrator.getInitialBiasZAsAcceleration(bz2);
28253 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28254 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28255 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28256 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28257 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28258 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28259 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28260 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28261 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28262 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28263 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28264 final double[] bias1 = calibrator.getInitialBias();
28265 assertArrayEquals(bias1, bias, 0.0);
28266 final double[] bias2 = new double[3];
28267 calibrator.getInitialBias(bias2);
28268 assertArrayEquals(bias1, bias2, 0.0);
28269 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28270 assertEquals(b1, ba);
28271 final Matrix b2 = new Matrix(3, 1);
28272 calibrator.getInitialBiasAsMatrix(b2);
28273 assertEquals(b1, b2);
28274 final Matrix ma1 = calibrator.getInitialMa();
28275 assertEquals(ma1, new Matrix(3, 3));
28276 final Matrix ma2 = new Matrix(3, 3);
28277 calibrator.getInitialMa(ma2);
28278 assertEquals(ma1, ma2);
28279 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28280 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28281 final NEDPosition nedPosition1 = new NEDPosition();
28282 assertTrue(calibrator.getNedPosition(nedPosition1));
28283 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28284 assertSame(calibrator.getMeasurements(), measurements);
28285 assertTrue(calibrator.isCommonAxisUsed());
28286 assertSame(calibrator.getListener(), this);
28287 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28288 assertFalse(calibrator.isReady());
28289 assertFalse(calibrator.isRunning());
28290 assertNull(calibrator.getEstimatedBiases());
28291 assertFalse(calibrator.getEstimatedBiases(null));
28292 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28293 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28294 assertNull(calibrator.getEstimatedBiasFx());
28295 assertNull(calibrator.getEstimatedBiasFy());
28296 assertNull(calibrator.getEstimatedBiasFz());
28297 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28298 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28299 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28300 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28301 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28302 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28303 assertNull(calibrator.getEstimatedMa());
28304 assertNull(calibrator.getEstimatedSx());
28305 assertNull(calibrator.getEstimatedSy());
28306 assertNull(calibrator.getEstimatedSz());
28307 assertNull(calibrator.getEstimatedMxy());
28308 assertNull(calibrator.getEstimatedMxz());
28309 assertNull(calibrator.getEstimatedMyx());
28310 assertNull(calibrator.getEstimatedMyz());
28311 assertNull(calibrator.getEstimatedMzx());
28312 assertNull(calibrator.getEstimatedMzy());
28313 assertNull(calibrator.getEstimatedCovariance());
28314 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28315 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28316 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28317 assertNotNull(calibrator.getGroundTruthGravityNorm());
28318 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28319 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28320 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28321 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28322 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28323 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28324 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28325
28326
28327 calibrator = null;
28328 try {
28329 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28330 measurements, true, new double[1], this);
28331 fail("IllegalArgumentException expected but not thrown");
28332 } catch (final IllegalArgumentException ignore) {
28333 }
28334 assertNull(calibrator);
28335 }
28336
28337 @Test
28338 public void testConstructor219() throws WrongSizeException {
28339 final Matrix ba = generateBa();
28340 final double[] bias = ba.getBuffer();
28341 final double biasX = ba.getElementAtIndex(0);
28342 final double biasY = ba.getElementAtIndex(1);
28343 final double biasZ = ba.getElementAtIndex(2);
28344
28345 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28346 final double latitude = Math.toRadians(
28347 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28348 final double longitude = Math.toRadians(
28349 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28350 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28351 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28352 final NEDVelocity nedVelocity = new NEDVelocity();
28353 final ECEFPosition ecefPosition = new ECEFPosition();
28354 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28355 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28356 ecefPosition, ecefVelocity);
28357
28358 KnownPositionAccelerometerCalibrator calibrator =
28359 new KnownPositionAccelerometerCalibrator(nedPosition, ba);
28360
28361
28362 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28363 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28364 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28365 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28366 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28367 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28368 final Acceleration bx2 = new Acceleration(0.0,
28369 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28370 calibrator.getInitialBiasXAsAcceleration(bx2);
28371 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28372 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28373 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28374 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28375 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28376 final Acceleration by2 = new Acceleration(0.0,
28377 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28378 calibrator.getInitialBiasYAsAcceleration(by2);
28379 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28380 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28381 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28382 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28383 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28384 final Acceleration bz2 = new Acceleration(0.0,
28385 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28386 calibrator.getInitialBiasZAsAcceleration(bz2);
28387 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28388 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28389 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28390 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28391 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28392 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28393 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28394 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28395 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28396 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28397 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28398 final double[] bias1 = calibrator.getInitialBias();
28399 assertArrayEquals(bias1, bias, 0.0);
28400 final double[] bias2 = new double[3];
28401 calibrator.getInitialBias(bias2);
28402 assertArrayEquals(bias1, bias2, 0.0);
28403 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28404 assertEquals(b1, ba);
28405 final Matrix b2 = new Matrix(3, 1);
28406 calibrator.getInitialBiasAsMatrix(b2);
28407 assertEquals(b1, b2);
28408 final Matrix ma1 = calibrator.getInitialMa();
28409 assertEquals(ma1, new Matrix(3, 3));
28410 final Matrix ma2 = new Matrix(3, 3);
28411 calibrator.getInitialMa(ma2);
28412 assertEquals(ma1, ma2);
28413 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28414 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28415 final NEDPosition nedPosition1 = new NEDPosition();
28416 assertTrue(calibrator.getNedPosition(nedPosition1));
28417 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28418 assertNull(calibrator.getMeasurements());
28419 assertFalse(calibrator.isCommonAxisUsed());
28420 assertNull(calibrator.getListener());
28421 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28422 assertFalse(calibrator.isReady());
28423 assertFalse(calibrator.isRunning());
28424 assertNull(calibrator.getEstimatedBiases());
28425 assertFalse(calibrator.getEstimatedBiases(null));
28426 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28427 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28428 assertNull(calibrator.getEstimatedBiasFx());
28429 assertNull(calibrator.getEstimatedBiasFy());
28430 assertNull(calibrator.getEstimatedBiasFz());
28431 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28432 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28433 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28434 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28435 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28436 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28437 assertNull(calibrator.getEstimatedMa());
28438 assertNull(calibrator.getEstimatedSx());
28439 assertNull(calibrator.getEstimatedSy());
28440 assertNull(calibrator.getEstimatedSz());
28441 assertNull(calibrator.getEstimatedMxy());
28442 assertNull(calibrator.getEstimatedMxz());
28443 assertNull(calibrator.getEstimatedMyx());
28444 assertNull(calibrator.getEstimatedMyz());
28445 assertNull(calibrator.getEstimatedMzx());
28446 assertNull(calibrator.getEstimatedMzy());
28447 assertNull(calibrator.getEstimatedCovariance());
28448 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28449 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28450 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28451 assertNotNull(calibrator.getGroundTruthGravityNorm());
28452 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28453 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28454 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28455 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28456 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28457 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28458 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28459
28460
28461 calibrator = null;
28462 try {
28463 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28464 new Matrix(1, 1));
28465 fail("IllegalArgumentException expected but not thrown");
28466 } catch (final IllegalArgumentException ignore) {
28467 }
28468 try {
28469 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28470 new Matrix(1, 3));
28471 fail("IllegalArgumentException expected but not thrown");
28472 } catch (final IllegalArgumentException ignore) {
28473 }
28474 assertNull(calibrator);
28475 }
28476
28477 @Test
28478 public void testConstructor220() throws WrongSizeException {
28479 final Matrix ba = generateBa();
28480 final double[] bias = ba.getBuffer();
28481 final double biasX = ba.getElementAtIndex(0);
28482 final double biasY = ba.getElementAtIndex(1);
28483 final double biasZ = ba.getElementAtIndex(2);
28484
28485 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28486 final double latitude = Math.toRadians(
28487 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28488 final double longitude = Math.toRadians(
28489 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28490 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28491 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28492 final NEDVelocity nedVelocity = new NEDVelocity();
28493 final ECEFPosition ecefPosition = new ECEFPosition();
28494 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28495 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28496 ecefPosition, ecefVelocity);
28497
28498 KnownPositionAccelerometerCalibrator calibrator =
28499 new KnownPositionAccelerometerCalibrator(nedPosition, ba,
28500 this);
28501
28502
28503 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28504 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28505 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28506 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28507 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28508 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28509 final Acceleration bx2 = new Acceleration(0.0,
28510 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28511 calibrator.getInitialBiasXAsAcceleration(bx2);
28512 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28513 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28514 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28515 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28516 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28517 final Acceleration by2 = new Acceleration(0.0,
28518 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28519 calibrator.getInitialBiasYAsAcceleration(by2);
28520 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28521 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28522 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28523 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28524 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28525 final Acceleration bz2 = new Acceleration(0.0,
28526 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28527 calibrator.getInitialBiasZAsAcceleration(bz2);
28528 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28529 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28530 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28531 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28532 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28533 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28534 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28535 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28536 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28537 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28538 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28539 final double[] bias1 = calibrator.getInitialBias();
28540 assertArrayEquals(bias1, bias, 0.0);
28541 final double[] bias2 = new double[3];
28542 calibrator.getInitialBias(bias2);
28543 assertArrayEquals(bias1, bias2, 0.0);
28544 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28545 assertEquals(b1, ba);
28546 final Matrix b2 = new Matrix(3, 1);
28547 calibrator.getInitialBiasAsMatrix(b2);
28548 assertEquals(b1, b2);
28549 final Matrix ma1 = calibrator.getInitialMa();
28550 assertEquals(ma1, new Matrix(3, 3));
28551 final Matrix ma2 = new Matrix(3, 3);
28552 calibrator.getInitialMa(ma2);
28553 assertEquals(ma1, ma2);
28554 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28555 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28556 final NEDPosition nedPosition1 = new NEDPosition();
28557 assertTrue(calibrator.getNedPosition(nedPosition1));
28558 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28559 assertNull(calibrator.getMeasurements());
28560 assertFalse(calibrator.isCommonAxisUsed());
28561 assertSame(calibrator.getListener(), this);
28562 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28563 assertFalse(calibrator.isReady());
28564 assertFalse(calibrator.isRunning());
28565 assertNull(calibrator.getEstimatedBiases());
28566 assertFalse(calibrator.getEstimatedBiases(null));
28567 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28568 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28569 assertNull(calibrator.getEstimatedBiasFx());
28570 assertNull(calibrator.getEstimatedBiasFy());
28571 assertNull(calibrator.getEstimatedBiasFz());
28572 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28573 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28574 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28575 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28576 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28577 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28578 assertNull(calibrator.getEstimatedMa());
28579 assertNull(calibrator.getEstimatedSx());
28580 assertNull(calibrator.getEstimatedSy());
28581 assertNull(calibrator.getEstimatedSz());
28582 assertNull(calibrator.getEstimatedMxy());
28583 assertNull(calibrator.getEstimatedMxz());
28584 assertNull(calibrator.getEstimatedMyx());
28585 assertNull(calibrator.getEstimatedMyz());
28586 assertNull(calibrator.getEstimatedMzx());
28587 assertNull(calibrator.getEstimatedMzy());
28588 assertNull(calibrator.getEstimatedCovariance());
28589 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28590 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28591 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28592 assertNotNull(calibrator.getGroundTruthGravityNorm());
28593 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28594 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28595 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28596 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28597 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28598 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28599 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28600
28601
28602 calibrator = null;
28603 try {
28604 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28605 new Matrix(1, 1), this);
28606 fail("IllegalArgumentException expected but not thrown");
28607 } catch (final IllegalArgumentException ignore) {
28608 }
28609 try {
28610 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28611 new Matrix(1, 3), this);
28612 fail("IllegalArgumentException expected but not thrown");
28613 } catch (final IllegalArgumentException ignore) {
28614 }
28615 assertNull(calibrator);
28616 }
28617
28618 @Test
28619 public void testConstructor221() throws WrongSizeException {
28620 final Collection<StandardDeviationBodyKinematics> measurements =
28621 Collections.emptyList();
28622
28623 final Matrix ba = generateBa();
28624 final double[] bias = ba.getBuffer();
28625 final double biasX = ba.getElementAtIndex(0);
28626 final double biasY = ba.getElementAtIndex(1);
28627 final double biasZ = ba.getElementAtIndex(2);
28628
28629 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28630 final double latitude = Math.toRadians(
28631 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28632 final double longitude = Math.toRadians(
28633 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28634 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28635 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28636 final NEDVelocity nedVelocity = new NEDVelocity();
28637 final ECEFPosition ecefPosition = new ECEFPosition();
28638 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28639 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28640 ecefPosition, ecefVelocity);
28641
28642 KnownPositionAccelerometerCalibrator calibrator =
28643 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28644 ba);
28645
28646
28647 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28648 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28649 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28650 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28651 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28652 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28653 final Acceleration bx2 = new Acceleration(0.0,
28654 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28655 calibrator.getInitialBiasXAsAcceleration(bx2);
28656 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28657 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28658 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28659 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28660 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28661 final Acceleration by2 = new Acceleration(0.0,
28662 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28663 calibrator.getInitialBiasYAsAcceleration(by2);
28664 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28665 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28666 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28667 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28668 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28669 final Acceleration bz2 = new Acceleration(0.0,
28670 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28671 calibrator.getInitialBiasZAsAcceleration(bz2);
28672 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28673 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28674 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28675 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28676 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28677 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28678 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28679 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28680 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28681 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28682 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28683 final double[] bias1 = calibrator.getInitialBias();
28684 assertArrayEquals(bias1, bias, 0.0);
28685 final double[] bias2 = new double[3];
28686 calibrator.getInitialBias(bias2);
28687 assertArrayEquals(bias1, bias2, 0.0);
28688 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28689 assertEquals(b1, ba);
28690 final Matrix b2 = new Matrix(3, 1);
28691 calibrator.getInitialBiasAsMatrix(b2);
28692 assertEquals(b1, b2);
28693 final Matrix ma1 = calibrator.getInitialMa();
28694 assertEquals(ma1, new Matrix(3, 3));
28695 final Matrix ma2 = new Matrix(3, 3);
28696 calibrator.getInitialMa(ma2);
28697 assertEquals(ma1, ma2);
28698 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28699 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28700 final NEDPosition nedPosition1 = new NEDPosition();
28701 assertTrue(calibrator.getNedPosition(nedPosition1));
28702 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28703 assertSame(calibrator.getMeasurements(), measurements);
28704 assertFalse(calibrator.isCommonAxisUsed());
28705 assertNull(calibrator.getListener());
28706 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28707 assertFalse(calibrator.isReady());
28708 assertFalse(calibrator.isRunning());
28709 assertNull(calibrator.getEstimatedBiases());
28710 assertFalse(calibrator.getEstimatedBiases(null));
28711 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28712 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28713 assertNull(calibrator.getEstimatedBiasFx());
28714 assertNull(calibrator.getEstimatedBiasFy());
28715 assertNull(calibrator.getEstimatedBiasFz());
28716 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28717 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28718 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28719 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28720 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28721 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28722 assertNull(calibrator.getEstimatedMa());
28723 assertNull(calibrator.getEstimatedSx());
28724 assertNull(calibrator.getEstimatedSy());
28725 assertNull(calibrator.getEstimatedSz());
28726 assertNull(calibrator.getEstimatedMxy());
28727 assertNull(calibrator.getEstimatedMxz());
28728 assertNull(calibrator.getEstimatedMyx());
28729 assertNull(calibrator.getEstimatedMyz());
28730 assertNull(calibrator.getEstimatedMzx());
28731 assertNull(calibrator.getEstimatedMzy());
28732 assertNull(calibrator.getEstimatedCovariance());
28733 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28734 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28735 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28736 assertNotNull(calibrator.getGroundTruthGravityNorm());
28737 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28738 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28739 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28740 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28741 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28742 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28743 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28744
28745
28746 calibrator = null;
28747 try {
28748 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28749 measurements, new Matrix(1, 1));
28750 fail("IllegalArgumentException expected but not thrown");
28751 } catch (final IllegalArgumentException ignore) {
28752 }
28753 try {
28754 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28755 measurements, new Matrix(1, 3));
28756 fail("IllegalArgumentException expected but not thrown");
28757 } catch (final IllegalArgumentException ignore) {
28758 }
28759 assertNull(calibrator);
28760 }
28761
28762 @Test
28763 public void testConstructor222() throws WrongSizeException {
28764 final Collection<StandardDeviationBodyKinematics> measurements =
28765 Collections.emptyList();
28766
28767 final Matrix ba = generateBa();
28768 final double[] bias = ba.getBuffer();
28769 final double biasX = ba.getElementAtIndex(0);
28770 final double biasY = ba.getElementAtIndex(1);
28771 final double biasZ = ba.getElementAtIndex(2);
28772
28773 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28774 final double latitude = Math.toRadians(
28775 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28776 final double longitude = Math.toRadians(
28777 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28778 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28779 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28780 final NEDVelocity nedVelocity = new NEDVelocity();
28781 final ECEFPosition ecefPosition = new ECEFPosition();
28782 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28783 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28784 ecefPosition, ecefVelocity);
28785
28786 KnownPositionAccelerometerCalibrator calibrator =
28787 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
28788 ba, this);
28789
28790
28791 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28792 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28793 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28794 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28795 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28796 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28797 final Acceleration bx2 = new Acceleration(0.0,
28798 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28799 calibrator.getInitialBiasXAsAcceleration(bx2);
28800 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28801 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28802 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28803 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28804 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28805 final Acceleration by2 = new Acceleration(0.0,
28806 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28807 calibrator.getInitialBiasYAsAcceleration(by2);
28808 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28809 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28810 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28811 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28812 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28813 final Acceleration bz2 = new Acceleration(0.0,
28814 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28815 calibrator.getInitialBiasZAsAcceleration(bz2);
28816 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28817 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28818 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28819 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28820 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28821 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28822 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28823 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28824 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28825 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28826 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28827 final double[] bias1 = calibrator.getInitialBias();
28828 assertArrayEquals(bias1, bias, 0.0);
28829 final double[] bias2 = new double[3];
28830 calibrator.getInitialBias(bias2);
28831 assertArrayEquals(bias1, bias2, 0.0);
28832 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28833 assertEquals(b1, ba);
28834 final Matrix b2 = new Matrix(3, 1);
28835 calibrator.getInitialBiasAsMatrix(b2);
28836 assertEquals(b1, b2);
28837 final Matrix ma1 = calibrator.getInitialMa();
28838 assertEquals(ma1, new Matrix(3, 3));
28839 final Matrix ma2 = new Matrix(3, 3);
28840 calibrator.getInitialMa(ma2);
28841 assertEquals(ma1, ma2);
28842 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28843 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28844 final NEDPosition nedPosition1 = new NEDPosition();
28845 assertTrue(calibrator.getNedPosition(nedPosition1));
28846 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28847 assertSame(calibrator.getMeasurements(), measurements);
28848 assertFalse(calibrator.isCommonAxisUsed());
28849 assertSame(calibrator.getListener(), this);
28850 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
28851 assertFalse(calibrator.isReady());
28852 assertFalse(calibrator.isRunning());
28853 assertNull(calibrator.getEstimatedBiases());
28854 assertFalse(calibrator.getEstimatedBiases(null));
28855 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28856 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28857 assertNull(calibrator.getEstimatedBiasFx());
28858 assertNull(calibrator.getEstimatedBiasFy());
28859 assertNull(calibrator.getEstimatedBiasFz());
28860 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
28861 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
28862 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
28863 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
28864 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
28865 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
28866 assertNull(calibrator.getEstimatedMa());
28867 assertNull(calibrator.getEstimatedSx());
28868 assertNull(calibrator.getEstimatedSy());
28869 assertNull(calibrator.getEstimatedSz());
28870 assertNull(calibrator.getEstimatedMxy());
28871 assertNull(calibrator.getEstimatedMxz());
28872 assertNull(calibrator.getEstimatedMyx());
28873 assertNull(calibrator.getEstimatedMyz());
28874 assertNull(calibrator.getEstimatedMzx());
28875 assertNull(calibrator.getEstimatedMzy());
28876 assertNull(calibrator.getEstimatedCovariance());
28877 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
28878 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
28879 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
28880 assertNotNull(calibrator.getGroundTruthGravityNorm());
28881 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
28882 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
28883 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
28884 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
28885 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
28886 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
28887 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
28888
28889
28890 calibrator = null;
28891 try {
28892 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28893 measurements, new Matrix(1, 1), this);
28894 fail("IllegalArgumentException expected but not thrown");
28895 } catch (final IllegalArgumentException ignore) {
28896 }
28897 try {
28898 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
28899 measurements, new Matrix(1, 3), this);
28900 fail("IllegalArgumentException expected but not thrown");
28901 } catch (final IllegalArgumentException ignore) {
28902 }
28903 assertNull(calibrator);
28904 }
28905
28906 @Test
28907 public void testConstructor223() throws WrongSizeException {
28908 final Matrix ba = generateBa();
28909 final double[] bias = ba.getBuffer();
28910 final double biasX = ba.getElementAtIndex(0);
28911 final double biasY = ba.getElementAtIndex(1);
28912 final double biasZ = ba.getElementAtIndex(2);
28913
28914 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
28915 final double latitude = Math.toRadians(
28916 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
28917 final double longitude = Math.toRadians(
28918 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
28919 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
28920 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
28921 final NEDVelocity nedVelocity = new NEDVelocity();
28922 final ECEFPosition ecefPosition = new ECEFPosition();
28923 final ECEFVelocity ecefVelocity = new ECEFVelocity();
28924 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
28925 ecefPosition, ecefVelocity);
28926
28927 KnownPositionAccelerometerCalibrator calibrator =
28928 new KnownPositionAccelerometerCalibrator(nedPosition,
28929 true, ba);
28930
28931
28932 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
28933 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
28934 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
28935 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
28936 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
28937 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28938 final Acceleration bx2 = new Acceleration(0.0,
28939 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28940 calibrator.getInitialBiasXAsAcceleration(bx2);
28941 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
28942 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28943 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
28944 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
28945 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28946 final Acceleration by2 = new Acceleration(0.0,
28947 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28948 calibrator.getInitialBiasYAsAcceleration(by2);
28949 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
28950 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28951 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
28952 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
28953 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28954 final Acceleration bz2 = new Acceleration(0.0,
28955 AccelerationUnit.FEET_PER_SQUARED_SECOND);
28956 calibrator.getInitialBiasZAsAcceleration(bz2);
28957 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
28958 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
28959 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
28960 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
28961 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
28962 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
28963 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
28964 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
28965 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
28966 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
28967 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
28968 final double[] bias1 = calibrator.getInitialBias();
28969 assertArrayEquals(bias1, bias, 0.0);
28970 final double[] bias2 = new double[3];
28971 calibrator.getInitialBias(bias2);
28972 assertArrayEquals(bias1, bias2, 0.0);
28973 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
28974 assertEquals(b1, ba);
28975 final Matrix b2 = new Matrix(3, 1);
28976 calibrator.getInitialBiasAsMatrix(b2);
28977 assertEquals(b1, b2);
28978 final Matrix ma1 = calibrator.getInitialMa();
28979 assertEquals(ma1, new Matrix(3, 3));
28980 final Matrix ma2 = new Matrix(3, 3);
28981 calibrator.getInitialMa(ma2);
28982 assertEquals(ma1, ma2);
28983 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
28984 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
28985 final NEDPosition nedPosition1 = new NEDPosition();
28986 assertTrue(calibrator.getNedPosition(nedPosition1));
28987 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
28988 assertNull(calibrator.getMeasurements());
28989 assertTrue(calibrator.isCommonAxisUsed());
28990 assertNull(calibrator.getListener());
28991 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
28992 assertFalse(calibrator.isReady());
28993 assertFalse(calibrator.isRunning());
28994 assertNull(calibrator.getEstimatedBiases());
28995 assertFalse(calibrator.getEstimatedBiases(null));
28996 assertNull(calibrator.getEstimatedBiasesAsMatrix());
28997 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
28998 assertNull(calibrator.getEstimatedBiasFx());
28999 assertNull(calibrator.getEstimatedBiasFy());
29000 assertNull(calibrator.getEstimatedBiasFz());
29001 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29002 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29003 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29004 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29005 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29006 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29007 assertNull(calibrator.getEstimatedMa());
29008 assertNull(calibrator.getEstimatedSx());
29009 assertNull(calibrator.getEstimatedSy());
29010 assertNull(calibrator.getEstimatedSz());
29011 assertNull(calibrator.getEstimatedMxy());
29012 assertNull(calibrator.getEstimatedMxz());
29013 assertNull(calibrator.getEstimatedMyx());
29014 assertNull(calibrator.getEstimatedMyz());
29015 assertNull(calibrator.getEstimatedMzx());
29016 assertNull(calibrator.getEstimatedMzy());
29017 assertNull(calibrator.getEstimatedCovariance());
29018 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29019 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29020 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29021 assertNotNull(calibrator.getGroundTruthGravityNorm());
29022 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29023 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29024 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29025 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29026 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29027 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29028 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29029
29030
29031 calibrator = null;
29032 try {
29033 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29034 true, new Matrix(1, 1));
29035 fail("IllegalArgumentException expected but not thrown");
29036 } catch (final IllegalArgumentException ignore) {
29037 }
29038 try {
29039 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29040 true, new Matrix(1, 3));
29041 fail("IllegalArgumentException expected but not thrown");
29042 } catch (final IllegalArgumentException ignore) {
29043 }
29044 assertNull(calibrator);
29045 }
29046
29047 @Test
29048 public void testConstructor224() throws WrongSizeException {
29049 final Matrix ba = generateBa();
29050 final double[] bias = ba.getBuffer();
29051 final double biasX = ba.getElementAtIndex(0);
29052 final double biasY = ba.getElementAtIndex(1);
29053 final double biasZ = ba.getElementAtIndex(2);
29054
29055 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29056 final double latitude = Math.toRadians(
29057 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29058 final double longitude = Math.toRadians(
29059 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29060 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29061 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29062 final NEDVelocity nedVelocity = new NEDVelocity();
29063 final ECEFPosition ecefPosition = new ECEFPosition();
29064 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29065 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29066 ecefPosition, ecefVelocity);
29067
29068 KnownPositionAccelerometerCalibrator calibrator =
29069 new KnownPositionAccelerometerCalibrator(nedPosition,
29070 true, ba, this);
29071
29072
29073 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29074 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29075 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29076 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29077 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29078 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29079 final Acceleration bx2 = new Acceleration(0.0,
29080 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29081 calibrator.getInitialBiasXAsAcceleration(bx2);
29082 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29083 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29084 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29085 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29086 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29087 final Acceleration by2 = new Acceleration(0.0,
29088 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29089 calibrator.getInitialBiasYAsAcceleration(by2);
29090 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29091 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29092 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29093 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29094 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29095 final Acceleration bz2 = new Acceleration(0.0,
29096 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29097 calibrator.getInitialBiasZAsAcceleration(bz2);
29098 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29099 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29100 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29101 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29102 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29103 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29104 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29105 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29106 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29107 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29108 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29109 final double[] bias1 = calibrator.getInitialBias();
29110 assertArrayEquals(bias1, bias, 0.0);
29111 final double[] bias2 = new double[3];
29112 calibrator.getInitialBias(bias2);
29113 assertArrayEquals(bias1, bias2, 0.0);
29114 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29115 assertEquals(b1, ba);
29116 final Matrix b2 = new Matrix(3, 1);
29117 calibrator.getInitialBiasAsMatrix(b2);
29118 assertEquals(b1, b2);
29119 final Matrix ma1 = calibrator.getInitialMa();
29120 assertEquals(ma1, new Matrix(3, 3));
29121 final Matrix ma2 = new Matrix(3, 3);
29122 calibrator.getInitialMa(ma2);
29123 assertEquals(ma1, ma2);
29124 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29125 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29126 final NEDPosition nedPosition1 = new NEDPosition();
29127 assertTrue(calibrator.getNedPosition(nedPosition1));
29128 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29129 assertNull(calibrator.getMeasurements());
29130 assertTrue(calibrator.isCommonAxisUsed());
29131 assertSame(calibrator.getListener(), this);
29132 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29133 assertFalse(calibrator.isReady());
29134 assertFalse(calibrator.isRunning());
29135 assertNull(calibrator.getEstimatedBiases());
29136 assertFalse(calibrator.getEstimatedBiases(null));
29137 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29138 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29139 assertNull(calibrator.getEstimatedBiasFx());
29140 assertNull(calibrator.getEstimatedBiasFy());
29141 assertNull(calibrator.getEstimatedBiasFz());
29142 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29143 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29144 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29145 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29146 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29147 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29148 assertNull(calibrator.getEstimatedMa());
29149 assertNull(calibrator.getEstimatedSx());
29150 assertNull(calibrator.getEstimatedSy());
29151 assertNull(calibrator.getEstimatedSz());
29152 assertNull(calibrator.getEstimatedMxy());
29153 assertNull(calibrator.getEstimatedMxz());
29154 assertNull(calibrator.getEstimatedMyx());
29155 assertNull(calibrator.getEstimatedMyz());
29156 assertNull(calibrator.getEstimatedMzx());
29157 assertNull(calibrator.getEstimatedMzy());
29158 assertNull(calibrator.getEstimatedCovariance());
29159 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29160 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29161 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29162 assertNotNull(calibrator.getGroundTruthGravityNorm());
29163 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29164 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29165 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29166 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29167 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29168 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29169 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29170
29171
29172 calibrator = null;
29173 try {
29174 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29175 true, new Matrix(1, 1),
29176 this);
29177 fail("IllegalArgumentException expected but not thrown");
29178 } catch (final IllegalArgumentException ignore) {
29179 }
29180 try {
29181 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29182 true, new Matrix(1, 3),
29183 this);
29184 fail("IllegalArgumentException expected but not thrown");
29185 } catch (final IllegalArgumentException ignore) {
29186 }
29187 assertNull(calibrator);
29188 }
29189
29190 @Test
29191 public void testConstructor225() throws WrongSizeException {
29192 final Collection<StandardDeviationBodyKinematics> measurements =
29193 Collections.emptyList();
29194
29195 final Matrix ba = generateBa();
29196 final double[] bias = ba.getBuffer();
29197 final double biasX = ba.getElementAtIndex(0);
29198 final double biasY = ba.getElementAtIndex(1);
29199 final double biasZ = ba.getElementAtIndex(2);
29200
29201 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29202 final double latitude = Math.toRadians(
29203 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29204 final double longitude = Math.toRadians(
29205 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29206 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29207 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29208 final NEDVelocity nedVelocity = new NEDVelocity();
29209 final ECEFPosition ecefPosition = new ECEFPosition();
29210 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29211 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29212 ecefPosition, ecefVelocity);
29213
29214 KnownPositionAccelerometerCalibrator calibrator =
29215 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29216 true, ba);
29217
29218
29219 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29220 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29221 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29222 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29223 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29224 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29225 final Acceleration bx2 = new Acceleration(0.0,
29226 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29227 calibrator.getInitialBiasXAsAcceleration(bx2);
29228 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29229 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29230 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29231 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29232 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29233 final Acceleration by2 = new Acceleration(0.0,
29234 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29235 calibrator.getInitialBiasYAsAcceleration(by2);
29236 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29237 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29238 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29239 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29240 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29241 final Acceleration bz2 = new Acceleration(0.0,
29242 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29243 calibrator.getInitialBiasZAsAcceleration(bz2);
29244 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29245 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29246 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29247 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29248 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29249 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29250 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29251 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29252 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29253 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29254 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29255 final double[] bias1 = calibrator.getInitialBias();
29256 assertArrayEquals(bias1, bias, 0.0);
29257 final double[] bias2 = new double[3];
29258 calibrator.getInitialBias(bias2);
29259 assertArrayEquals(bias1, bias2, 0.0);
29260 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29261 assertEquals(b1, ba);
29262 final Matrix b2 = new Matrix(3, 1);
29263 calibrator.getInitialBiasAsMatrix(b2);
29264 assertEquals(b1, b2);
29265 final Matrix ma1 = calibrator.getInitialMa();
29266 assertEquals(ma1, new Matrix(3, 3));
29267 final Matrix ma2 = new Matrix(3, 3);
29268 calibrator.getInitialMa(ma2);
29269 assertEquals(ma1, ma2);
29270 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29271 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29272 final NEDPosition nedPosition1 = new NEDPosition();
29273 assertTrue(calibrator.getNedPosition(nedPosition1));
29274 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29275 assertSame(calibrator.getMeasurements(), measurements);
29276 assertTrue(calibrator.isCommonAxisUsed());
29277 assertNull(calibrator.getListener());
29278 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29279 assertFalse(calibrator.isReady());
29280 assertFalse(calibrator.isRunning());
29281 assertNull(calibrator.getEstimatedBiases());
29282 assertFalse(calibrator.getEstimatedBiases(null));
29283 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29284 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29285 assertNull(calibrator.getEstimatedBiasFx());
29286 assertNull(calibrator.getEstimatedBiasFy());
29287 assertNull(calibrator.getEstimatedBiasFz());
29288 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29289 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29290 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29291 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29292 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29293 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29294 assertNull(calibrator.getEstimatedMa());
29295 assertNull(calibrator.getEstimatedSx());
29296 assertNull(calibrator.getEstimatedSy());
29297 assertNull(calibrator.getEstimatedSz());
29298 assertNull(calibrator.getEstimatedMxy());
29299 assertNull(calibrator.getEstimatedMxz());
29300 assertNull(calibrator.getEstimatedMyx());
29301 assertNull(calibrator.getEstimatedMyz());
29302 assertNull(calibrator.getEstimatedMzx());
29303 assertNull(calibrator.getEstimatedMzy());
29304 assertNull(calibrator.getEstimatedCovariance());
29305 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29306 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29307 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29308 assertNotNull(calibrator.getGroundTruthGravityNorm());
29309 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29310 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29311 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29312 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29313 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29314 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29315 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29316
29317
29318 calibrator = null;
29319 try {
29320 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29321 measurements, true,
29322 new Matrix(1, 1));
29323 fail("IllegalArgumentException expected but not thrown");
29324 } catch (final IllegalArgumentException ignore) {
29325 }
29326 try {
29327 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29328 measurements, true,
29329 new Matrix(1, 3));
29330 fail("IllegalArgumentException expected but not thrown");
29331 } catch (final IllegalArgumentException ignore) {
29332 }
29333 assertNull(calibrator);
29334 }
29335
29336 @Test
29337 public void testConstructor226() throws WrongSizeException {
29338 final Collection<StandardDeviationBodyKinematics> measurements =
29339 Collections.emptyList();
29340
29341 final Matrix ba = generateBa();
29342 final double[] bias = ba.getBuffer();
29343 final double biasX = ba.getElementAtIndex(0);
29344 final double biasY = ba.getElementAtIndex(1);
29345 final double biasZ = ba.getElementAtIndex(2);
29346
29347 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29348 final double latitude = Math.toRadians(
29349 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29350 final double longitude = Math.toRadians(
29351 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29352 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29353 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29354 final NEDVelocity nedVelocity = new NEDVelocity();
29355 final ECEFPosition ecefPosition = new ECEFPosition();
29356 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29357 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29358 ecefPosition, ecefVelocity);
29359
29360 KnownPositionAccelerometerCalibrator calibrator =
29361 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
29362 true, ba, this);
29363
29364
29365 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29366 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29367 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29368 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29369 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29370 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29371 final Acceleration bx2 = new Acceleration(0.0,
29372 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29373 calibrator.getInitialBiasXAsAcceleration(bx2);
29374 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29375 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29376 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29377 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29378 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29379 final Acceleration by2 = new Acceleration(0.0,
29380 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29381 calibrator.getInitialBiasYAsAcceleration(by2);
29382 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29383 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29384 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29385 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29386 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29387 final Acceleration bz2 = new Acceleration(0.0,
29388 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29389 calibrator.getInitialBiasZAsAcceleration(bz2);
29390 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29391 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29392 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
29393 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
29394 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
29395 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
29396 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
29397 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
29398 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
29399 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
29400 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
29401 final double[] bias1 = calibrator.getInitialBias();
29402 assertArrayEquals(bias1, bias, 0.0);
29403 final double[] bias2 = new double[3];
29404 calibrator.getInitialBias(bias2);
29405 assertArrayEquals(bias1, bias2, 0.0);
29406 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29407 assertEquals(b1, ba);
29408 final Matrix b2 = new Matrix(3, 1);
29409 calibrator.getInitialBiasAsMatrix(b2);
29410 assertEquals(b1, b2);
29411 final Matrix ma1 = calibrator.getInitialMa();
29412 assertEquals(ma1, new Matrix(3, 3));
29413 final Matrix ma2 = new Matrix(3, 3);
29414 calibrator.getInitialMa(ma2);
29415 assertEquals(ma1, ma2);
29416 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29417 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29418 final NEDPosition nedPosition1 = new NEDPosition();
29419 assertTrue(calibrator.getNedPosition(nedPosition1));
29420 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29421 assertSame(calibrator.getMeasurements(), measurements);
29422 assertTrue(calibrator.isCommonAxisUsed());
29423 assertSame(calibrator.getListener(), this);
29424 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
29425 assertFalse(calibrator.isReady());
29426 assertFalse(calibrator.isRunning());
29427 assertNull(calibrator.getEstimatedBiases());
29428 assertFalse(calibrator.getEstimatedBiases(null));
29429 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29430 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29431 assertNull(calibrator.getEstimatedBiasFx());
29432 assertNull(calibrator.getEstimatedBiasFy());
29433 assertNull(calibrator.getEstimatedBiasFz());
29434 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29435 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29436 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29437 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29438 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29439 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29440 assertNull(calibrator.getEstimatedMa());
29441 assertNull(calibrator.getEstimatedSx());
29442 assertNull(calibrator.getEstimatedSy());
29443 assertNull(calibrator.getEstimatedSz());
29444 assertNull(calibrator.getEstimatedMxy());
29445 assertNull(calibrator.getEstimatedMxz());
29446 assertNull(calibrator.getEstimatedMyx());
29447 assertNull(calibrator.getEstimatedMyz());
29448 assertNull(calibrator.getEstimatedMzx());
29449 assertNull(calibrator.getEstimatedMzy());
29450 assertNull(calibrator.getEstimatedCovariance());
29451 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29452 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29453 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29454 assertNotNull(calibrator.getGroundTruthGravityNorm());
29455 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29456 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29457 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29458 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29459 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29460 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29461 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29462
29463
29464 calibrator = null;
29465 try {
29466 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29467 measurements, true,
29468 new Matrix(1, 1), this);
29469 fail("IllegalArgumentException expected but not thrown");
29470 } catch (final IllegalArgumentException ignore) {
29471 }
29472 try {
29473 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29474 measurements, true,
29475 new Matrix(1, 3), this);
29476 fail("IllegalArgumentException expected but not thrown");
29477 } catch (final IllegalArgumentException ignore) {
29478 }
29479 assertNull(calibrator);
29480 }
29481
29482 @Test
29483 public void testConstructor227() throws WrongSizeException {
29484 final Matrix ba = generateBa();
29485 final double[] bias = ba.getBuffer();
29486 final double biasX = ba.getElementAtIndex(0);
29487 final double biasY = ba.getElementAtIndex(1);
29488 final double biasZ = ba.getElementAtIndex(2);
29489
29490 final Matrix ma = generateMaCommonAxis();
29491 final double sx = ma.getElementAt(0, 0);
29492 final double sy = ma.getElementAt(1, 1);
29493 final double sz = ma.getElementAt(2, 2);
29494 final double mxy = ma.getElementAt(0, 1);
29495 final double mxz = ma.getElementAt(0, 2);
29496 final double myx = ma.getElementAt(1, 0);
29497 final double myz = ma.getElementAt(1, 2);
29498 final double mzx = ma.getElementAt(2, 0);
29499 final double mzy = ma.getElementAt(2, 1);
29500
29501 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29502 final double latitude = Math.toRadians(
29503 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29504 final double longitude = Math.toRadians(
29505 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29506 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29507 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29508 final NEDVelocity nedVelocity = new NEDVelocity();
29509 final ECEFPosition ecefPosition = new ECEFPosition();
29510 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29511 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29512 ecefPosition, ecefVelocity);
29513
29514 KnownPositionAccelerometerCalibrator calibrator =
29515 new KnownPositionAccelerometerCalibrator(nedPosition, ba, ma);
29516
29517
29518 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29519 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29520 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29521 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29522 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29523 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29524 final Acceleration bx2 = new Acceleration(0.0,
29525 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29526 calibrator.getInitialBiasXAsAcceleration(bx2);
29527 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29528 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29529 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29530 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29531 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29532 final Acceleration by2 = new Acceleration(0.0,
29533 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29534 calibrator.getInitialBiasYAsAcceleration(by2);
29535 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29536 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29537 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29538 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29539 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29540 final Acceleration bz2 = new Acceleration(0.0,
29541 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29542 calibrator.getInitialBiasZAsAcceleration(bz2);
29543 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29544 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29545 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29546 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29547 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29548 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29549 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29550 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29551 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29552 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29553 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29554 final double[] bias1 = calibrator.getInitialBias();
29555 assertArrayEquals(bias1, bias, 0.0);
29556 final double[] bias2 = new double[3];
29557 calibrator.getInitialBias(bias2);
29558 assertArrayEquals(bias1, bias2, 0.0);
29559 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29560 assertEquals(b1, ba);
29561 final Matrix b2 = new Matrix(3, 1);
29562 calibrator.getInitialBiasAsMatrix(b2);
29563 assertEquals(b1, b2);
29564 final Matrix ma1 = new Matrix(3, 3);
29565 ma1.setSubmatrix(0, 0,
29566 2, 2,
29567 new double[]{sx, myx, mzx,
29568 mxy, sy, mzy,
29569 mxz, myz, sz});
29570 assertEquals(calibrator.getInitialMa(), ma1);
29571 final Matrix ma2 = new Matrix(3, 3);
29572 calibrator.getInitialMa(ma2);
29573 assertEquals(ma1, ma2);
29574 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29575 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29576 final NEDPosition nedPosition1 = new NEDPosition();
29577 assertTrue(calibrator.getNedPosition(nedPosition1));
29578 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29579 assertNull(calibrator.getMeasurements());
29580 assertFalse(calibrator.isCommonAxisUsed());
29581 assertNull(calibrator.getListener());
29582 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29583 assertFalse(calibrator.isReady());
29584 assertFalse(calibrator.isRunning());
29585 assertNull(calibrator.getEstimatedBiases());
29586 assertFalse(calibrator.getEstimatedBiases(null));
29587 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29588 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29589 assertNull(calibrator.getEstimatedBiasFx());
29590 assertNull(calibrator.getEstimatedBiasFy());
29591 assertNull(calibrator.getEstimatedBiasFz());
29592 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29593 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29594 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29595 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29596 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29597 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29598 assertNull(calibrator.getEstimatedMa());
29599 assertNull(calibrator.getEstimatedSx());
29600 assertNull(calibrator.getEstimatedSy());
29601 assertNull(calibrator.getEstimatedSz());
29602 assertNull(calibrator.getEstimatedMxy());
29603 assertNull(calibrator.getEstimatedMxz());
29604 assertNull(calibrator.getEstimatedMyx());
29605 assertNull(calibrator.getEstimatedMyz());
29606 assertNull(calibrator.getEstimatedMzx());
29607 assertNull(calibrator.getEstimatedMzy());
29608 assertNull(calibrator.getEstimatedCovariance());
29609 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29610 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29611 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29612 assertNotNull(calibrator.getGroundTruthGravityNorm());
29613 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29614 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29615 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29616 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29617 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29618 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29619 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29620
29621
29622 calibrator = null;
29623 try {
29624 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29625 new Matrix(1, 1), ma);
29626 fail("IllegalArgumentException expected but not thrown");
29627 } catch (final IllegalArgumentException ignore) {
29628 }
29629 try {
29630 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29631 new Matrix(1, 3), ma);
29632 fail("IllegalArgumentException expected but not thrown");
29633 } catch (final IllegalArgumentException ignore) {
29634 }
29635 try {
29636 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29637 ba, new Matrix(1, 3));
29638 fail("IllegalArgumentException expected but not thrown");
29639 } catch (final IllegalArgumentException ignore) {
29640 }
29641 try {
29642 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29643 ba, new Matrix(3, 1));
29644 fail("IllegalArgumentException expected but not thrown");
29645 } catch (final IllegalArgumentException ignore) {
29646 }
29647 assertNull(calibrator);
29648 }
29649
29650 @Test
29651 public void testConstructor228() throws WrongSizeException {
29652 final Matrix ba = generateBa();
29653 final double[] bias = ba.getBuffer();
29654 final double biasX = ba.getElementAtIndex(0);
29655 final double biasY = ba.getElementAtIndex(1);
29656 final double biasZ = ba.getElementAtIndex(2);
29657
29658 final Matrix ma = generateMaCommonAxis();
29659 final double sx = ma.getElementAt(0, 0);
29660 final double sy = ma.getElementAt(1, 1);
29661 final double sz = ma.getElementAt(2, 2);
29662 final double mxy = ma.getElementAt(0, 1);
29663 final double mxz = ma.getElementAt(0, 2);
29664 final double myx = ma.getElementAt(1, 0);
29665 final double myz = ma.getElementAt(1, 2);
29666 final double mzx = ma.getElementAt(2, 0);
29667 final double mzy = ma.getElementAt(2, 1);
29668
29669 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29670 final double latitude = Math.toRadians(
29671 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29672 final double longitude = Math.toRadians(
29673 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29674 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29675 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29676 final NEDVelocity nedVelocity = new NEDVelocity();
29677 final ECEFPosition ecefPosition = new ECEFPosition();
29678 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29679 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29680 ecefPosition, ecefVelocity);
29681
29682 KnownPositionAccelerometerCalibrator calibrator =
29683 new KnownPositionAccelerometerCalibrator(nedPosition,
29684 ba, ma, this);
29685
29686
29687 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29688 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29689 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29690 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29691 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29692 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29693 final Acceleration bx2 = new Acceleration(0.0,
29694 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29695 calibrator.getInitialBiasXAsAcceleration(bx2);
29696 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29697 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29698 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29699 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29700 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29701 final Acceleration by2 = new Acceleration(0.0,
29702 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29703 calibrator.getInitialBiasYAsAcceleration(by2);
29704 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29705 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29706 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29707 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29708 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29709 final Acceleration bz2 = new Acceleration(0.0,
29710 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29711 calibrator.getInitialBiasZAsAcceleration(bz2);
29712 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29713 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29714 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29715 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29716 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29717 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29718 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29719 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29720 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29721 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29722 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29723 final double[] bias1 = calibrator.getInitialBias();
29724 assertArrayEquals(bias1, bias, 0.0);
29725 final double[] bias2 = new double[3];
29726 calibrator.getInitialBias(bias2);
29727 assertArrayEquals(bias1, bias2, 0.0);
29728 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29729 assertEquals(b1, ba);
29730 final Matrix b2 = new Matrix(3, 1);
29731 calibrator.getInitialBiasAsMatrix(b2);
29732 assertEquals(b1, b2);
29733 final Matrix ma1 = new Matrix(3, 3);
29734 ma1.setSubmatrix(0, 0,
29735 2, 2,
29736 new double[]{sx, myx, mzx,
29737 mxy, sy, mzy,
29738 mxz, myz, sz});
29739 assertEquals(calibrator.getInitialMa(), ma1);
29740 final Matrix ma2 = new Matrix(3, 3);
29741 calibrator.getInitialMa(ma2);
29742 assertEquals(ma1, ma2);
29743 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29744 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29745 final NEDPosition nedPosition1 = new NEDPosition();
29746 assertTrue(calibrator.getNedPosition(nedPosition1));
29747 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29748 assertNull(calibrator.getMeasurements());
29749 assertFalse(calibrator.isCommonAxisUsed());
29750 assertSame(calibrator.getListener(), this);
29751 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29752 assertFalse(calibrator.isReady());
29753 assertFalse(calibrator.isRunning());
29754 assertNull(calibrator.getEstimatedBiases());
29755 assertFalse(calibrator.getEstimatedBiases(null));
29756 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29757 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29758 assertNull(calibrator.getEstimatedBiasFx());
29759 assertNull(calibrator.getEstimatedBiasFy());
29760 assertNull(calibrator.getEstimatedBiasFz());
29761 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29762 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29763 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29764 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29765 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29766 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29767 assertNull(calibrator.getEstimatedMa());
29768 assertNull(calibrator.getEstimatedSx());
29769 assertNull(calibrator.getEstimatedSy());
29770 assertNull(calibrator.getEstimatedSz());
29771 assertNull(calibrator.getEstimatedMxy());
29772 assertNull(calibrator.getEstimatedMxz());
29773 assertNull(calibrator.getEstimatedMyx());
29774 assertNull(calibrator.getEstimatedMyz());
29775 assertNull(calibrator.getEstimatedMzx());
29776 assertNull(calibrator.getEstimatedMzy());
29777 assertNull(calibrator.getEstimatedCovariance());
29778 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29779 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29780 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29781 assertNotNull(calibrator.getGroundTruthGravityNorm());
29782 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29783 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29784 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29785 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29786 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29787 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29788 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29789
29790
29791 calibrator = null;
29792 try {
29793 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29794 new Matrix(1, 1), ma, this);
29795 fail("IllegalArgumentException expected but not thrown");
29796 } catch (final IllegalArgumentException ignore) {
29797 }
29798 try {
29799 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29800 new Matrix(1, 3), ma, this);
29801 fail("IllegalArgumentException expected but not thrown");
29802 } catch (final IllegalArgumentException ignore) {
29803 }
29804 try {
29805 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29806 ba, new Matrix(1, 3), this);
29807 fail("IllegalArgumentException expected but not thrown");
29808 } catch (final IllegalArgumentException ignore) {
29809 }
29810 try {
29811 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29812 ba, new Matrix(3, 1), this);
29813 fail("IllegalArgumentException expected but not thrown");
29814 } catch (final IllegalArgumentException ignore) {
29815 }
29816 assertNull(calibrator);
29817 }
29818
29819 @Test
29820 public void testConstructor229() throws WrongSizeException {
29821 final Collection<StandardDeviationBodyKinematics> measurements =
29822 Collections.emptyList();
29823
29824 final Matrix ba = generateBa();
29825 final double[] bias = ba.getBuffer();
29826 final double biasX = ba.getElementAtIndex(0);
29827 final double biasY = ba.getElementAtIndex(1);
29828 final double biasZ = ba.getElementAtIndex(2);
29829
29830 final Matrix ma = generateMaCommonAxis();
29831 final double sx = ma.getElementAt(0, 0);
29832 final double sy = ma.getElementAt(1, 1);
29833 final double sz = ma.getElementAt(2, 2);
29834 final double mxy = ma.getElementAt(0, 1);
29835 final double mxz = ma.getElementAt(0, 2);
29836 final double myx = ma.getElementAt(1, 0);
29837 final double myz = ma.getElementAt(1, 2);
29838 final double mzx = ma.getElementAt(2, 0);
29839 final double mzy = ma.getElementAt(2, 1);
29840
29841 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
29842 final double latitude = Math.toRadians(
29843 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
29844 final double longitude = Math.toRadians(
29845 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
29846 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
29847 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
29848 final NEDVelocity nedVelocity = new NEDVelocity();
29849 final ECEFPosition ecefPosition = new ECEFPosition();
29850 final ECEFVelocity ecefVelocity = new ECEFVelocity();
29851 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
29852 ecefPosition, ecefVelocity);
29853
29854 KnownPositionAccelerometerCalibrator calibrator =
29855 new KnownPositionAccelerometerCalibrator(nedPosition,
29856 measurements, ba, ma);
29857
29858
29859 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
29860 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
29861 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
29862 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
29863 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
29864 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29865 final Acceleration bx2 = new Acceleration(0.0,
29866 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29867 calibrator.getInitialBiasXAsAcceleration(bx2);
29868 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
29869 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29870 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
29871 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
29872 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29873 final Acceleration by2 = new Acceleration(0.0,
29874 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29875 calibrator.getInitialBiasYAsAcceleration(by2);
29876 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
29877 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29878 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
29879 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
29880 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29881 final Acceleration bz2 = new Acceleration(0.0,
29882 AccelerationUnit.FEET_PER_SQUARED_SECOND);
29883 calibrator.getInitialBiasZAsAcceleration(bz2);
29884 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
29885 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
29886 assertEquals(calibrator.getInitialSx(), sx, 0.0);
29887 assertEquals(calibrator.getInitialSy(), sy, 0.0);
29888 assertEquals(calibrator.getInitialSz(), sz, 0.0);
29889 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
29890 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
29891 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
29892 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
29893 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
29894 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
29895 final double[] bias1 = calibrator.getInitialBias();
29896 assertArrayEquals(bias1, bias, 0.0);
29897 final double[] bias2 = new double[3];
29898 calibrator.getInitialBias(bias2);
29899 assertArrayEquals(bias1, bias2, 0.0);
29900 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
29901 assertEquals(b1, ba);
29902 final Matrix b2 = new Matrix(3, 1);
29903 calibrator.getInitialBiasAsMatrix(b2);
29904 assertEquals(b1, b2);
29905 final Matrix ma1 = new Matrix(3, 3);
29906 ma1.setSubmatrix(0, 0,
29907 2, 2,
29908 new double[]{sx, myx, mzx,
29909 mxy, sy, mzy,
29910 mxz, myz, sz});
29911 assertEquals(calibrator.getInitialMa(), ma1);
29912 final Matrix ma2 = new Matrix(3, 3);
29913 calibrator.getInitialMa(ma2);
29914 assertEquals(ma1, ma2);
29915 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
29916 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
29917 final NEDPosition nedPosition1 = new NEDPosition();
29918 assertTrue(calibrator.getNedPosition(nedPosition1));
29919 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
29920 assertSame(calibrator.getMeasurements(), measurements);
29921 assertFalse(calibrator.isCommonAxisUsed());
29922 assertNull(calibrator.getListener());
29923 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
29924 assertFalse(calibrator.isReady());
29925 assertFalse(calibrator.isRunning());
29926 assertNull(calibrator.getEstimatedBiases());
29927 assertFalse(calibrator.getEstimatedBiases(null));
29928 assertNull(calibrator.getEstimatedBiasesAsMatrix());
29929 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
29930 assertNull(calibrator.getEstimatedBiasFx());
29931 assertNull(calibrator.getEstimatedBiasFy());
29932 assertNull(calibrator.getEstimatedBiasFz());
29933 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
29934 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
29935 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
29936 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
29937 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
29938 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
29939 assertNull(calibrator.getEstimatedMa());
29940 assertNull(calibrator.getEstimatedSx());
29941 assertNull(calibrator.getEstimatedSy());
29942 assertNull(calibrator.getEstimatedSz());
29943 assertNull(calibrator.getEstimatedMxy());
29944 assertNull(calibrator.getEstimatedMxz());
29945 assertNull(calibrator.getEstimatedMyx());
29946 assertNull(calibrator.getEstimatedMyz());
29947 assertNull(calibrator.getEstimatedMzx());
29948 assertNull(calibrator.getEstimatedMzy());
29949 assertNull(calibrator.getEstimatedCovariance());
29950 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
29951 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
29952 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
29953 assertNotNull(calibrator.getGroundTruthGravityNorm());
29954 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
29955 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
29956 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
29957 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
29958 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
29959 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
29960 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
29961
29962
29963 calibrator = null;
29964 try {
29965 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29966 measurements, new Matrix(1, 1), ma);
29967 fail("IllegalArgumentException expected but not thrown");
29968 } catch (final IllegalArgumentException ignore) {
29969 }
29970 try {
29971 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29972 measurements, new Matrix(1, 3), ma);
29973 fail("IllegalArgumentException expected but not thrown");
29974 } catch (final IllegalArgumentException ignore) {
29975 }
29976 try {
29977 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29978 measurements, ba, new Matrix(1, 3));
29979 fail("IllegalArgumentException expected but not thrown");
29980 } catch (final IllegalArgumentException ignore) {
29981 }
29982 try {
29983 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
29984 measurements, ba, new Matrix(3, 1));
29985 fail("IllegalArgumentException expected but not thrown");
29986 } catch (final IllegalArgumentException ignore) {
29987 }
29988 assertNull(calibrator);
29989 }
29990
29991 @Test
29992 public void testConstructor230() throws WrongSizeException {
29993 final Collection<StandardDeviationBodyKinematics> measurements =
29994 Collections.emptyList();
29995
29996 final Matrix ba = generateBa();
29997 final double[] bias = ba.getBuffer();
29998 final double biasX = ba.getElementAtIndex(0);
29999 final double biasY = ba.getElementAtIndex(1);
30000 final double biasZ = ba.getElementAtIndex(2);
30001
30002 final Matrix ma = generateMaCommonAxis();
30003 final double sx = ma.getElementAt(0, 0);
30004 final double sy = ma.getElementAt(1, 1);
30005 final double sz = ma.getElementAt(2, 2);
30006 final double mxy = ma.getElementAt(0, 1);
30007 final double mxz = ma.getElementAt(0, 2);
30008 final double myx = ma.getElementAt(1, 0);
30009 final double myz = ma.getElementAt(1, 2);
30010 final double mzx = ma.getElementAt(2, 0);
30011 final double mzy = ma.getElementAt(2, 1);
30012
30013 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30014 final double latitude = Math.toRadians(
30015 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30016 final double longitude = Math.toRadians(
30017 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30018 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30019 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30020 final NEDVelocity nedVelocity = new NEDVelocity();
30021 final ECEFPosition ecefPosition = new ECEFPosition();
30022 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30023 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30024 ecefPosition, ecefVelocity);
30025
30026 KnownPositionAccelerometerCalibrator calibrator =
30027 new KnownPositionAccelerometerCalibrator(nedPosition,
30028 measurements, ba, ma, this);
30029
30030
30031 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30032 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30033 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30034 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30035 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30036 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30037 final Acceleration bx2 = new Acceleration(0.0,
30038 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30039 calibrator.getInitialBiasXAsAcceleration(bx2);
30040 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30041 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30042 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30043 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30044 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30045 final Acceleration by2 = new Acceleration(0.0,
30046 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30047 calibrator.getInitialBiasYAsAcceleration(by2);
30048 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30049 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30050 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30051 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30052 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30053 final Acceleration bz2 = new Acceleration(0.0,
30054 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30055 calibrator.getInitialBiasZAsAcceleration(bz2);
30056 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30057 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30058 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30059 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30060 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30061 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30062 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30063 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30064 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30065 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30066 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30067 final double[] bias1 = calibrator.getInitialBias();
30068 assertArrayEquals(bias1, bias, 0.0);
30069 final double[] bias2 = new double[3];
30070 calibrator.getInitialBias(bias2);
30071 assertArrayEquals(bias1, bias2, 0.0);
30072 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30073 assertEquals(b1, ba);
30074 final Matrix b2 = new Matrix(3, 1);
30075 calibrator.getInitialBiasAsMatrix(b2);
30076 assertEquals(b1, b2);
30077 final Matrix ma1 = new Matrix(3, 3);
30078 ma1.setSubmatrix(0, 0,
30079 2, 2,
30080 new double[]{sx, myx, mzx,
30081 mxy, sy, mzy,
30082 mxz, myz, sz});
30083 assertEquals(calibrator.getInitialMa(), ma1);
30084 final Matrix ma2 = new Matrix(3, 3);
30085 calibrator.getInitialMa(ma2);
30086 assertEquals(ma1, ma2);
30087 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30088 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30089 final NEDPosition nedPosition1 = new NEDPosition();
30090 assertTrue(calibrator.getNedPosition(nedPosition1));
30091 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30092 assertSame(calibrator.getMeasurements(), measurements);
30093 assertFalse(calibrator.isCommonAxisUsed());
30094 assertSame(calibrator.getListener(), this);
30095 assertEquals(calibrator.getMinimumRequiredMeasurements(), 13);
30096 assertFalse(calibrator.isReady());
30097 assertFalse(calibrator.isRunning());
30098 assertNull(calibrator.getEstimatedBiases());
30099 assertFalse(calibrator.getEstimatedBiases(null));
30100 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30101 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30102 assertNull(calibrator.getEstimatedBiasFx());
30103 assertNull(calibrator.getEstimatedBiasFy());
30104 assertNull(calibrator.getEstimatedBiasFz());
30105 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30106 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30107 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30108 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30109 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30110 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30111 assertNull(calibrator.getEstimatedMa());
30112 assertNull(calibrator.getEstimatedSx());
30113 assertNull(calibrator.getEstimatedSy());
30114 assertNull(calibrator.getEstimatedSz());
30115 assertNull(calibrator.getEstimatedMxy());
30116 assertNull(calibrator.getEstimatedMxz());
30117 assertNull(calibrator.getEstimatedMyx());
30118 assertNull(calibrator.getEstimatedMyz());
30119 assertNull(calibrator.getEstimatedMzx());
30120 assertNull(calibrator.getEstimatedMzy());
30121 assertNull(calibrator.getEstimatedCovariance());
30122 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30123 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30124 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30125 assertNotNull(calibrator.getGroundTruthGravityNorm());
30126 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30127 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30128 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30129 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30130 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30131 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30132 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30133
30134
30135 calibrator = null;
30136 try {
30137 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30138 measurements, new Matrix(1, 1), ma, this);
30139 fail("IllegalArgumentException expected but not thrown");
30140 } catch (final IllegalArgumentException ignore) {
30141 }
30142 try {
30143 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30144 measurements, new Matrix(1, 3), ma, this);
30145 fail("IllegalArgumentException expected but not thrown");
30146 } catch (final IllegalArgumentException ignore) {
30147 }
30148 try {
30149 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30150 measurements, ba, new Matrix(1, 3), this);
30151 fail("IllegalArgumentException expected but not thrown");
30152 } catch (final IllegalArgumentException ignore) {
30153 }
30154 try {
30155 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30156 measurements, ba, new Matrix(3, 1), this);
30157 fail("IllegalArgumentException expected but not thrown");
30158 } catch (final IllegalArgumentException ignore) {
30159 }
30160 assertNull(calibrator);
30161 }
30162
30163 @Test
30164 public void testConstructor231() throws WrongSizeException {
30165 final Matrix ba = generateBa();
30166 final double[] bias = ba.getBuffer();
30167 final double biasX = ba.getElementAtIndex(0);
30168 final double biasY = ba.getElementAtIndex(1);
30169 final double biasZ = ba.getElementAtIndex(2);
30170
30171 final Matrix ma = generateMaCommonAxis();
30172 final double sx = ma.getElementAt(0, 0);
30173 final double sy = ma.getElementAt(1, 1);
30174 final double sz = ma.getElementAt(2, 2);
30175 final double mxy = ma.getElementAt(0, 1);
30176 final double mxz = ma.getElementAt(0, 2);
30177 final double myx = ma.getElementAt(1, 0);
30178 final double myz = ma.getElementAt(1, 2);
30179 final double mzx = ma.getElementAt(2, 0);
30180 final double mzy = ma.getElementAt(2, 1);
30181
30182 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30183 final double latitude = Math.toRadians(
30184 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30185 final double longitude = Math.toRadians(
30186 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30187 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30188 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30189 final NEDVelocity nedVelocity = new NEDVelocity();
30190 final ECEFPosition ecefPosition = new ECEFPosition();
30191 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30192 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30193 ecefPosition, ecefVelocity);
30194
30195 KnownPositionAccelerometerCalibrator calibrator =
30196 new KnownPositionAccelerometerCalibrator(nedPosition,
30197 true, ba, ma);
30198
30199
30200 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30201 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30202 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30203 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30204 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30205 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30206 final Acceleration bx2 = new Acceleration(0.0,
30207 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30208 calibrator.getInitialBiasXAsAcceleration(bx2);
30209 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30210 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30211 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30212 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30213 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30214 final Acceleration by2 = new Acceleration(0.0,
30215 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30216 calibrator.getInitialBiasYAsAcceleration(by2);
30217 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30218 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30219 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30220 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30221 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30222 final Acceleration bz2 = new Acceleration(0.0,
30223 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30224 calibrator.getInitialBiasZAsAcceleration(bz2);
30225 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30226 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30227 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30228 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30229 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30230 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30231 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30232 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30233 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30234 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30235 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30236 final double[] bias1 = calibrator.getInitialBias();
30237 assertArrayEquals(bias1, bias, 0.0);
30238 final double[] bias2 = new double[3];
30239 calibrator.getInitialBias(bias2);
30240 assertArrayEquals(bias1, bias2, 0.0);
30241 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30242 assertEquals(b1, ba);
30243 final Matrix b2 = new Matrix(3, 1);
30244 calibrator.getInitialBiasAsMatrix(b2);
30245 assertEquals(b1, b2);
30246 final Matrix ma1 = new Matrix(3, 3);
30247 ma1.setSubmatrix(0, 0,
30248 2, 2,
30249 new double[]{sx, myx, mzx,
30250 mxy, sy, mzy,
30251 mxz, myz, sz});
30252 assertEquals(calibrator.getInitialMa(), ma1);
30253 final Matrix ma2 = new Matrix(3, 3);
30254 calibrator.getInitialMa(ma2);
30255 assertEquals(ma1, ma2);
30256 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30257 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30258 final NEDPosition nedPosition1 = new NEDPosition();
30259 assertTrue(calibrator.getNedPosition(nedPosition1));
30260 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30261 assertNull(calibrator.getMeasurements());
30262 assertTrue(calibrator.isCommonAxisUsed());
30263 assertNull(calibrator.getListener());
30264 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30265 assertFalse(calibrator.isReady());
30266 assertFalse(calibrator.isRunning());
30267 assertNull(calibrator.getEstimatedBiases());
30268 assertFalse(calibrator.getEstimatedBiases(null));
30269 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30270 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30271 assertNull(calibrator.getEstimatedBiasFx());
30272 assertNull(calibrator.getEstimatedBiasFy());
30273 assertNull(calibrator.getEstimatedBiasFz());
30274 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30275 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30276 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30277 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30278 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30279 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30280 assertNull(calibrator.getEstimatedMa());
30281 assertNull(calibrator.getEstimatedSx());
30282 assertNull(calibrator.getEstimatedSy());
30283 assertNull(calibrator.getEstimatedSz());
30284 assertNull(calibrator.getEstimatedMxy());
30285 assertNull(calibrator.getEstimatedMxz());
30286 assertNull(calibrator.getEstimatedMyx());
30287 assertNull(calibrator.getEstimatedMyz());
30288 assertNull(calibrator.getEstimatedMzx());
30289 assertNull(calibrator.getEstimatedMzy());
30290 assertNull(calibrator.getEstimatedCovariance());
30291 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30292 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30293 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30294 assertNotNull(calibrator.getGroundTruthGravityNorm());
30295 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30296 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30297 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30298 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30299 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30300 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30301 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30302
30303
30304 calibrator = null;
30305 try {
30306 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30307 true, new Matrix(1, 1), ma);
30308 fail("IllegalArgumentException expected but not thrown");
30309 } catch (final IllegalArgumentException ignore) {
30310 }
30311 try {
30312 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30313 true, new Matrix(1, 3), ma);
30314 fail("IllegalArgumentException expected but not thrown");
30315 } catch (final IllegalArgumentException ignore) {
30316 }
30317 try {
30318 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30319 true, ba, new Matrix(1, 3));
30320 fail("IllegalArgumentException expected but not thrown");
30321 } catch (final IllegalArgumentException ignore) {
30322 }
30323 try {
30324 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30325 true, ba, new Matrix(3, 1));
30326 fail("IllegalArgumentException expected but not thrown");
30327 } catch (final IllegalArgumentException ignore) {
30328 }
30329 assertNull(calibrator);
30330 }
30331
30332 @Test
30333 public void testConstructor232() throws WrongSizeException {
30334 final Matrix ba = generateBa();
30335 final double[] bias = ba.getBuffer();
30336 final double biasX = ba.getElementAtIndex(0);
30337 final double biasY = ba.getElementAtIndex(1);
30338 final double biasZ = ba.getElementAtIndex(2);
30339
30340 final Matrix ma = generateMaCommonAxis();
30341 final double sx = ma.getElementAt(0, 0);
30342 final double sy = ma.getElementAt(1, 1);
30343 final double sz = ma.getElementAt(2, 2);
30344 final double mxy = ma.getElementAt(0, 1);
30345 final double mxz = ma.getElementAt(0, 2);
30346 final double myx = ma.getElementAt(1, 0);
30347 final double myz = ma.getElementAt(1, 2);
30348 final double mzx = ma.getElementAt(2, 0);
30349 final double mzy = ma.getElementAt(2, 1);
30350
30351 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30352 final double latitude = Math.toRadians(
30353 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30354 final double longitude = Math.toRadians(
30355 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30356 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30357 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30358 final NEDVelocity nedVelocity = new NEDVelocity();
30359 final ECEFPosition ecefPosition = new ECEFPosition();
30360 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30361 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30362 ecefPosition, ecefVelocity);
30363
30364 KnownPositionAccelerometerCalibrator calibrator =
30365 new KnownPositionAccelerometerCalibrator(nedPosition,
30366 true, ba, ma, this);
30367
30368
30369 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30370 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30371 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30372 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30373 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30374 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30375 final Acceleration bx2 = new Acceleration(0.0,
30376 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30377 calibrator.getInitialBiasXAsAcceleration(bx2);
30378 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30379 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30380 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30381 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30382 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30383 final Acceleration by2 = new Acceleration(0.0,
30384 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30385 calibrator.getInitialBiasYAsAcceleration(by2);
30386 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30387 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30388 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30389 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30390 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30391 final Acceleration bz2 = new Acceleration(0.0,
30392 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30393 calibrator.getInitialBiasZAsAcceleration(bz2);
30394 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30395 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30396 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30397 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30398 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30399 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30400 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30401 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30402 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30403 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30404 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30405 final double[] bias1 = calibrator.getInitialBias();
30406 assertArrayEquals(bias1, bias, 0.0);
30407 final double[] bias2 = new double[3];
30408 calibrator.getInitialBias(bias2);
30409 assertArrayEquals(bias1, bias2, 0.0);
30410 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30411 assertEquals(b1, ba);
30412 final Matrix b2 = new Matrix(3, 1);
30413 calibrator.getInitialBiasAsMatrix(b2);
30414 assertEquals(b1, b2);
30415 final Matrix ma1 = new Matrix(3, 3);
30416 ma1.setSubmatrix(0, 0,
30417 2, 2,
30418 new double[]{sx, myx, mzx,
30419 mxy, sy, mzy,
30420 mxz, myz, sz});
30421 assertEquals(calibrator.getInitialMa(), ma1);
30422 final Matrix ma2 = new Matrix(3, 3);
30423 calibrator.getInitialMa(ma2);
30424 assertEquals(ma1, ma2);
30425 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30426 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30427 final NEDPosition nedPosition1 = new NEDPosition();
30428 assertTrue(calibrator.getNedPosition(nedPosition1));
30429 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30430 assertNull(calibrator.getMeasurements());
30431 assertTrue(calibrator.isCommonAxisUsed());
30432 assertSame(calibrator.getListener(), this);
30433 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30434 assertFalse(calibrator.isReady());
30435 assertFalse(calibrator.isRunning());
30436 assertNull(calibrator.getEstimatedBiases());
30437 assertFalse(calibrator.getEstimatedBiases(null));
30438 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30439 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30440 assertNull(calibrator.getEstimatedBiasFx());
30441 assertNull(calibrator.getEstimatedBiasFy());
30442 assertNull(calibrator.getEstimatedBiasFz());
30443 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30444 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30445 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30446 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30447 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30448 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30449 assertNull(calibrator.getEstimatedMa());
30450 assertNull(calibrator.getEstimatedSx());
30451 assertNull(calibrator.getEstimatedSy());
30452 assertNull(calibrator.getEstimatedSz());
30453 assertNull(calibrator.getEstimatedMxy());
30454 assertNull(calibrator.getEstimatedMxz());
30455 assertNull(calibrator.getEstimatedMyx());
30456 assertNull(calibrator.getEstimatedMyz());
30457 assertNull(calibrator.getEstimatedMzx());
30458 assertNull(calibrator.getEstimatedMzy());
30459 assertNull(calibrator.getEstimatedCovariance());
30460 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30461 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30462 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30463 assertNotNull(calibrator.getGroundTruthGravityNorm());
30464 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30465 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30466 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30467 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30468 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30469 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30470 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30471
30472
30473 calibrator = null;
30474 try {
30475 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30476 true, new Matrix(1, 1), ma, this);
30477 fail("IllegalArgumentException expected but not thrown");
30478 } catch (final IllegalArgumentException ignore) {
30479 }
30480 try {
30481 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30482 true, new Matrix(1, 3), ma, this);
30483 fail("IllegalArgumentException expected but not thrown");
30484 } catch (final IllegalArgumentException ignore) {
30485 }
30486 try {
30487 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30488 true, ba, new Matrix(1, 3), this);
30489 fail("IllegalArgumentException expected but not thrown");
30490 } catch (final IllegalArgumentException ignore) {
30491 }
30492 try {
30493 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30494 true, ba, new Matrix(3, 1), this);
30495 fail("IllegalArgumentException expected but not thrown");
30496 } catch (final IllegalArgumentException ignore) {
30497 }
30498 assertNull(calibrator);
30499 }
30500
30501 @Test
30502 public void testConstructor233() throws WrongSizeException {
30503 final Collection<StandardDeviationBodyKinematics> measurements =
30504 Collections.emptyList();
30505
30506 final Matrix ba = generateBa();
30507 final double[] bias = ba.getBuffer();
30508 final double biasX = ba.getElementAtIndex(0);
30509 final double biasY = ba.getElementAtIndex(1);
30510 final double biasZ = ba.getElementAtIndex(2);
30511
30512 final Matrix ma = generateMaCommonAxis();
30513 final double sx = ma.getElementAt(0, 0);
30514 final double sy = ma.getElementAt(1, 1);
30515 final double sz = ma.getElementAt(2, 2);
30516 final double mxy = ma.getElementAt(0, 1);
30517 final double mxz = ma.getElementAt(0, 2);
30518 final double myx = ma.getElementAt(1, 0);
30519 final double myz = ma.getElementAt(1, 2);
30520 final double mzx = ma.getElementAt(2, 0);
30521 final double mzy = ma.getElementAt(2, 1);
30522
30523 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30524 final double latitude = Math.toRadians(
30525 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30526 final double longitude = Math.toRadians(
30527 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30528 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30529 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30530 final NEDVelocity nedVelocity = new NEDVelocity();
30531 final ECEFPosition ecefPosition = new ECEFPosition();
30532 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30533 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30534 ecefPosition, ecefVelocity);
30535
30536 KnownPositionAccelerometerCalibrator calibrator =
30537 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30538 true, ba, ma);
30539
30540
30541 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30542 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30543 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30544 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30545 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30546 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30547 final Acceleration bx2 = new Acceleration(0.0,
30548 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30549 calibrator.getInitialBiasXAsAcceleration(bx2);
30550 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30551 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30552 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30553 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30554 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30555 final Acceleration by2 = new Acceleration(0.0,
30556 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30557 calibrator.getInitialBiasYAsAcceleration(by2);
30558 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30559 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30560 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30561 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30562 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30563 final Acceleration bz2 = new Acceleration(0.0,
30564 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30565 calibrator.getInitialBiasZAsAcceleration(bz2);
30566 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30567 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30568 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30569 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30570 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30571 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30572 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30573 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30574 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30575 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30576 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30577 final double[] bias1 = calibrator.getInitialBias();
30578 assertArrayEquals(bias1, bias, 0.0);
30579 final double[] bias2 = new double[3];
30580 calibrator.getInitialBias(bias2);
30581 assertArrayEquals(bias1, bias2, 0.0);
30582 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30583 assertEquals(b1, ba);
30584 final Matrix b2 = new Matrix(3, 1);
30585 calibrator.getInitialBiasAsMatrix(b2);
30586 assertEquals(b1, b2);
30587 final Matrix ma1 = new Matrix(3, 3);
30588 ma1.setSubmatrix(0, 0,
30589 2, 2,
30590 new double[]{sx, myx, mzx,
30591 mxy, sy, mzy,
30592 mxz, myz, sz});
30593 assertEquals(calibrator.getInitialMa(), ma1);
30594 final Matrix ma2 = new Matrix(3, 3);
30595 calibrator.getInitialMa(ma2);
30596 assertEquals(ma1, ma2);
30597 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30598 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30599 final NEDPosition nedPosition1 = new NEDPosition();
30600 assertTrue(calibrator.getNedPosition(nedPosition1));
30601 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30602 assertSame(calibrator.getMeasurements(), measurements);
30603 assertTrue(calibrator.isCommonAxisUsed());
30604 assertNull(calibrator.getListener());
30605 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30606 assertFalse(calibrator.isReady());
30607 assertFalse(calibrator.isRunning());
30608 assertNull(calibrator.getEstimatedBiases());
30609 assertFalse(calibrator.getEstimatedBiases(null));
30610 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30611 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30612 assertNull(calibrator.getEstimatedBiasFx());
30613 assertNull(calibrator.getEstimatedBiasFy());
30614 assertNull(calibrator.getEstimatedBiasFz());
30615 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30616 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30617 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30618 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30619 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30620 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30621 assertNull(calibrator.getEstimatedMa());
30622 assertNull(calibrator.getEstimatedSx());
30623 assertNull(calibrator.getEstimatedSy());
30624 assertNull(calibrator.getEstimatedSz());
30625 assertNull(calibrator.getEstimatedMxy());
30626 assertNull(calibrator.getEstimatedMxz());
30627 assertNull(calibrator.getEstimatedMyx());
30628 assertNull(calibrator.getEstimatedMyz());
30629 assertNull(calibrator.getEstimatedMzx());
30630 assertNull(calibrator.getEstimatedMzy());
30631 assertNull(calibrator.getEstimatedCovariance());
30632 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30633 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30634 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30635 assertNotNull(calibrator.getGroundTruthGravityNorm());
30636 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30637 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30638 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30639 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30640 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30641 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30642 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30643
30644
30645 calibrator = null;
30646 try {
30647 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30648 measurements, true,
30649 new Matrix(1, 1), ma);
30650 fail("IllegalArgumentException expected but not thrown");
30651 } catch (final IllegalArgumentException ignore) {
30652 }
30653 try {
30654 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30655 measurements, true,
30656 new Matrix(1, 3), ma);
30657 fail("IllegalArgumentException expected but not thrown");
30658 } catch (final IllegalArgumentException ignore) {
30659 }
30660 try {
30661 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30662 measurements, true,
30663 ba, new Matrix(1, 3));
30664 fail("IllegalArgumentException expected but not thrown");
30665 } catch (final IllegalArgumentException ignore) {
30666 }
30667 try {
30668 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30669 measurements, true,
30670 ba, new Matrix(3, 1));
30671 fail("IllegalArgumentException expected but not thrown");
30672 } catch (final IllegalArgumentException ignore) {
30673 }
30674 assertNull(calibrator);
30675 }
30676
30677 @Test
30678 public void testConstructor234() throws WrongSizeException {
30679 final Collection<StandardDeviationBodyKinematics> measurements =
30680 Collections.emptyList();
30681
30682 final Matrix ba = generateBa();
30683 final double[] bias = ba.getBuffer();
30684 final double biasX = ba.getElementAtIndex(0);
30685 final double biasY = ba.getElementAtIndex(1);
30686 final double biasZ = ba.getElementAtIndex(2);
30687
30688 final Matrix ma = generateMaCommonAxis();
30689 final double sx = ma.getElementAt(0, 0);
30690 final double sy = ma.getElementAt(1, 1);
30691 final double sz = ma.getElementAt(2, 2);
30692 final double mxy = ma.getElementAt(0, 1);
30693 final double mxz = ma.getElementAt(0, 2);
30694 final double myx = ma.getElementAt(1, 0);
30695 final double myz = ma.getElementAt(1, 2);
30696 final double mzx = ma.getElementAt(2, 0);
30697 final double mzy = ma.getElementAt(2, 1);
30698
30699 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
30700 final double latitude = Math.toRadians(
30701 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
30702 final double longitude = Math.toRadians(
30703 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
30704 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
30705 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
30706 final NEDVelocity nedVelocity = new NEDVelocity();
30707 final ECEFPosition ecefPosition = new ECEFPosition();
30708 final ECEFVelocity ecefVelocity = new ECEFVelocity();
30709 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
30710 ecefPosition, ecefVelocity);
30711
30712 KnownPositionAccelerometerCalibrator calibrator =
30713 new KnownPositionAccelerometerCalibrator(nedPosition, measurements,
30714 true, ba, ma, this);
30715
30716
30717 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30718 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30719 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30720 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30721 assertEquals(bx1.getValue().doubleValue(), biasX, 0.0);
30722 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30723 final Acceleration bx2 = new Acceleration(0.0,
30724 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30725 calibrator.getInitialBiasXAsAcceleration(bx2);
30726 assertEquals(bx2.getValue().doubleValue(), biasX, 0.0);
30727 assertEquals(bx2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30728 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30729 assertEquals(by1.getValue().doubleValue(), biasY, 0.0);
30730 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30731 final Acceleration by2 = new Acceleration(0.0,
30732 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30733 calibrator.getInitialBiasYAsAcceleration(by2);
30734 assertEquals(by2.getValue().doubleValue(), biasY, 0.0);
30735 assertEquals(by2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30736 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30737 assertEquals(bz1.getValue().doubleValue(), biasZ, 0.0);
30738 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30739 final Acceleration bz2 = new Acceleration(0.0,
30740 AccelerationUnit.FEET_PER_SQUARED_SECOND);
30741 calibrator.getInitialBiasZAsAcceleration(bz2);
30742 assertEquals(bz2.getValue().doubleValue(), biasZ, 0.0);
30743 assertEquals(bz2.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30744 assertEquals(calibrator.getInitialSx(), sx, 0.0);
30745 assertEquals(calibrator.getInitialSy(), sy, 0.0);
30746 assertEquals(calibrator.getInitialSz(), sz, 0.0);
30747 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
30748 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
30749 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
30750 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
30751 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
30752 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
30753 final double[] bias1 = calibrator.getInitialBias();
30754 assertArrayEquals(bias1, bias, 0.0);
30755 final double[] bias2 = new double[3];
30756 calibrator.getInitialBias(bias2);
30757 assertArrayEquals(bias1, bias2, 0.0);
30758 final Matrix b1 = calibrator.getInitialBiasAsMatrix();
30759 assertEquals(b1, ba);
30760 final Matrix b2 = new Matrix(3, 1);
30761 calibrator.getInitialBiasAsMatrix(b2);
30762 assertEquals(b1, b2);
30763 final Matrix ma1 = new Matrix(3, 3);
30764 ma1.setSubmatrix(0, 0,
30765 2, 2,
30766 new double[]{sx, myx, mzx,
30767 mxy, sy, mzy,
30768 mxz, myz, sz});
30769 assertEquals(calibrator.getInitialMa(), ma1);
30770 final Matrix ma2 = new Matrix(3, 3);
30771 calibrator.getInitialMa(ma2);
30772 assertEquals(ma1, ma2);
30773 assertTrue(calibrator.getEcefPosition().equals(ecefPosition, ABSOLUTE_ERROR));
30774 assertTrue(calibrator.getNedPosition().equals(nedPosition, ABSOLUTE_ERROR));
30775 final NEDPosition nedPosition1 = new NEDPosition();
30776 assertTrue(calibrator.getNedPosition(nedPosition1));
30777 assertTrue(nedPosition.equals(nedPosition1, ABSOLUTE_ERROR));
30778 assertSame(calibrator.getMeasurements(), measurements);
30779 assertTrue(calibrator.isCommonAxisUsed());
30780 assertSame(calibrator.getListener(), this);
30781 assertEquals(calibrator.getMinimumRequiredMeasurements(), 10);
30782 assertFalse(calibrator.isReady());
30783 assertFalse(calibrator.isRunning());
30784 assertNull(calibrator.getEstimatedBiases());
30785 assertFalse(calibrator.getEstimatedBiases(null));
30786 assertNull(calibrator.getEstimatedBiasesAsMatrix());
30787 assertFalse(calibrator.getEstimatedBiasesAsMatrix(null));
30788 assertNull(calibrator.getEstimatedBiasFx());
30789 assertNull(calibrator.getEstimatedBiasFy());
30790 assertNull(calibrator.getEstimatedBiasFz());
30791 assertNull(calibrator.getEstimatedBiasFxAsAcceleration());
30792 assertFalse(calibrator.getEstimatedBiasFxAsAcceleration(null));
30793 assertNull(calibrator.getEstimatedBiasFyAsAcceleration());
30794 assertFalse(calibrator.getEstimatedBiasFyAsAcceleration(null));
30795 assertNull(calibrator.getEstimatedBiasFzAsAcceleration());
30796 assertFalse(calibrator.getEstimatedBiasFzAsAcceleration(null));
30797 assertNull(calibrator.getEstimatedMa());
30798 assertNull(calibrator.getEstimatedSx());
30799 assertNull(calibrator.getEstimatedSy());
30800 assertNull(calibrator.getEstimatedSz());
30801 assertNull(calibrator.getEstimatedMxy());
30802 assertNull(calibrator.getEstimatedMxz());
30803 assertNull(calibrator.getEstimatedMyx());
30804 assertNull(calibrator.getEstimatedMyz());
30805 assertNull(calibrator.getEstimatedMzx());
30806 assertNull(calibrator.getEstimatedMzy());
30807 assertNull(calibrator.getEstimatedCovariance());
30808 assertEquals(calibrator.getEstimatedChiSq(), 0.0, 0.0);
30809 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(
30810 ecefPosition.getX(), ecefPosition.getY(), ecefPosition.getZ());
30811 assertNotNull(calibrator.getGroundTruthGravityNorm());
30812 assertEquals(gravity.getNorm(), calibrator.getGroundTruthGravityNorm(), ABSOLUTE_ERROR);
30813 assertNotNull(calibrator.getGroundTruthGravityNormAsAcceleration());
30814 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration()
30815 .equals(gravity.getNormAsAcceleration(), ABSOLUTE_ERROR));
30816 final Acceleration g = new Acceleration(0.0, AccelerationUnit.G);
30817 assertTrue(calibrator.getGroundTruthGravityNormAsAcceleration(g));
30818 assertTrue(gravity.getNormAsAcceleration().equals(g, ABSOLUTE_ERROR));
30819
30820
30821 calibrator = null;
30822 try {
30823 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30824 measurements, true,
30825 new Matrix(1, 1), ma, this);
30826 fail("IllegalArgumentException expected but not thrown");
30827 } catch (final IllegalArgumentException ignore) {
30828 }
30829 try {
30830 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30831 measurements, true,
30832 new Matrix(1, 3), ma, this);
30833 fail("IllegalArgumentException expected but not thrown");
30834 } catch (final IllegalArgumentException ignore) {
30835 }
30836 try {
30837 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30838 measurements, true,
30839 ba, new Matrix(1, 3), this);
30840 fail("IllegalArgumentException expected but not thrown");
30841 } catch (final IllegalArgumentException ignore) {
30842 }
30843 try {
30844 calibrator = new KnownPositionAccelerometerCalibrator(nedPosition,
30845 measurements, true,
30846 ba, new Matrix(3, 1), this);
30847 fail("IllegalArgumentException expected but not thrown");
30848 } catch (final IllegalArgumentException ignore) {
30849 }
30850 assertNull(calibrator);
30851 }
30852
30853 @Test
30854 public void testGetSetInitialBiasX() throws LockedException {
30855 final KnownPositionAccelerometerCalibrator calibrator =
30856 new KnownPositionAccelerometerCalibrator();
30857
30858 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30859
30860
30861 final Matrix ba = generateBa();
30862 final double biasX = ba.getElementAtIndex(0);
30863
30864 calibrator.setInitialBiasX(biasX);
30865
30866
30867 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
30868 }
30869
30870 @Test
30871 public void testGetSetInitialBiasY() throws LockedException {
30872 final KnownPositionAccelerometerCalibrator calibrator =
30873 new KnownPositionAccelerometerCalibrator();
30874
30875 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30876
30877
30878 final Matrix ba = generateBa();
30879 final double biasY = ba.getElementAtIndex(1);
30880
30881 calibrator.setInitialBiasY(biasY);
30882
30883
30884 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
30885 }
30886
30887 @Test
30888 public void testGetSetInitialBiasZ() throws LockedException {
30889 final KnownPositionAccelerometerCalibrator calibrator =
30890 new KnownPositionAccelerometerCalibrator();
30891
30892 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30893
30894
30895 final Matrix ba = generateBa();
30896 final double biasZ = ba.getElementAtIndex(2);
30897
30898 calibrator.setInitialBiasZ(biasZ);
30899
30900
30901 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
30902 }
30903
30904 @Test
30905 public void testGetSetInitialBiasXAsAcceleration() throws LockedException {
30906 final KnownPositionAccelerometerCalibrator calibrator =
30907 new KnownPositionAccelerometerCalibrator();
30908
30909
30910 final Acceleration bx1 = calibrator.getInitialBiasXAsAcceleration();
30911
30912 assertEquals(bx1.getValue().doubleValue(), 0.0, 0.0);
30913 assertEquals(bx1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30914
30915
30916 final Matrix ba = generateBa();
30917 final double biasX = ba.getElementAtIndex(0);
30918
30919 final Acceleration bx2 = new Acceleration(biasX,
30920 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30921 calibrator.setInitialBiasX(bx2);
30922
30923
30924 final Acceleration bx3 = calibrator.getInitialBiasXAsAcceleration();
30925 final Acceleration bx4 = new Acceleration(biasX,
30926 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30927 calibrator.getInitialBiasXAsAcceleration(bx4);
30928
30929 assertEquals(bx3, bx4);
30930 }
30931
30932 @Test
30933 public void testGetSetInitialBiasYAsAcceleration() throws LockedException {
30934 final KnownPositionAccelerometerCalibrator calibrator =
30935 new KnownPositionAccelerometerCalibrator();
30936
30937
30938 final Acceleration by1 = calibrator.getInitialBiasYAsAcceleration();
30939
30940 assertEquals(by1.getValue().doubleValue(), 0.0, 0.0);
30941 assertEquals(by1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30942
30943
30944 final Matrix ba = generateBa();
30945 final double biasY = ba.getElementAtIndex(1);
30946
30947 final Acceleration by2 = new Acceleration(biasY,
30948 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30949 calibrator.setInitialBiasY(by2);
30950
30951
30952 final Acceleration by3 = calibrator.getInitialBiasYAsAcceleration();
30953 final Acceleration by4 = new Acceleration(biasY,
30954 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30955 calibrator.getInitialBiasYAsAcceleration(by4);
30956
30957 assertEquals(by3, by4);
30958 }
30959
30960 @Test
30961 public void testGetSetInitialBiasZAsAcceleration() throws LockedException {
30962 final KnownPositionAccelerometerCalibrator calibrator =
30963 new KnownPositionAccelerometerCalibrator();
30964
30965
30966 final Acceleration bz1 = calibrator.getInitialBiasZAsAcceleration();
30967
30968 assertEquals(bz1.getValue().doubleValue(), 0.0, 0.0);
30969 assertEquals(bz1.getUnit(), AccelerationUnit.METERS_PER_SQUARED_SECOND);
30970
30971
30972 final Matrix ba = generateBa();
30973 final double biasZ = ba.getElementAtIndex(2);
30974
30975 final Acceleration bz2 = new Acceleration(biasZ,
30976 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30977 calibrator.setInitialBiasZ(bz2);
30978
30979
30980 final Acceleration bz3 = calibrator.getInitialBiasZAsAcceleration();
30981 final Acceleration bz4 = new Acceleration(biasZ,
30982 AccelerationUnit.METERS_PER_SQUARED_SECOND);
30983 calibrator.getInitialBiasZAsAcceleration(bz4);
30984
30985 assertEquals(bz3, bz4);
30986 }
30987
30988 @Test
30989 public void testSetInitialBias1() throws LockedException {
30990 final KnownPositionAccelerometerCalibrator calibrator =
30991 new KnownPositionAccelerometerCalibrator();
30992
30993
30994 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
30995 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
30996 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
30997
30998
30999 final Matrix ba = generateBa();
31000 final double biasX = ba.getElementAtIndex(0);
31001 final double biasY = ba.getElementAtIndex(1);
31002 final double biasZ = ba.getElementAtIndex(2);
31003
31004 calibrator.setInitialBias(biasX, biasY, biasZ);
31005
31006
31007 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31008 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31009 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31010 }
31011
31012 @Test
31013 public void testSetInitialBias2() throws LockedException {
31014 final KnownPositionAccelerometerCalibrator calibrator =
31015 new KnownPositionAccelerometerCalibrator();
31016
31017
31018 assertEquals(calibrator.getInitialBiasX(), 0.0, 0.0);
31019 assertEquals(calibrator.getInitialBiasY(), 0.0, 0.0);
31020 assertEquals(calibrator.getInitialBiasZ(), 0.0, 0.0);
31021
31022
31023 final Matrix ba = generateBa();
31024 final double biasX = ba.getElementAtIndex(0);
31025 final double biasY = ba.getElementAtIndex(1);
31026 final double biasZ = ba.getElementAtIndex(2);
31027
31028 final Acceleration bx = new Acceleration(biasX,
31029 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31030 final Acceleration by = new Acceleration(biasY,
31031 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31032 final Acceleration bz = new Acceleration(biasZ,
31033 AccelerationUnit.METERS_PER_SQUARED_SECOND);
31034
31035 calibrator.setInitialBias(bx, by, bz);
31036
31037
31038 assertEquals(calibrator.getInitialBiasX(), biasX, 0.0);
31039 assertEquals(calibrator.getInitialBiasY(), biasY, 0.0);
31040 assertEquals(calibrator.getInitialBiasZ(), biasZ, 0.0);
31041 }
31042
31043 @Test
31044 public void testGetSetInitialSx() throws WrongSizeException, LockedException {
31045 final KnownPositionAccelerometerCalibrator calibrator =
31046 new KnownPositionAccelerometerCalibrator();
31047
31048 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31049
31050
31051 final Matrix ma = generateMaGeneral();
31052 final double sx = ma.getElementAt(0, 0);
31053
31054 calibrator.setInitialSx(sx);
31055
31056
31057 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31058 }
31059
31060 @Test
31061 public void testGetSetInitialSy() throws WrongSizeException, LockedException {
31062 final KnownPositionAccelerometerCalibrator calibrator =
31063 new KnownPositionAccelerometerCalibrator();
31064
31065 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31066
31067
31068 final Matrix ma = generateMaGeneral();
31069 final double sy = ma.getElementAt(1, 1);
31070
31071 calibrator.setInitialSy(sy);
31072
31073
31074 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31075 }
31076
31077 @Test
31078 public void testGetSetInitialSz() throws WrongSizeException, LockedException {
31079 final KnownPositionAccelerometerCalibrator calibrator =
31080 new KnownPositionAccelerometerCalibrator();
31081
31082 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31083
31084
31085 final Matrix ma = generateMaGeneral();
31086 final double sz = ma.getElementAt(2, 2);
31087
31088 calibrator.setInitialSz(sz);
31089
31090
31091 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31092 }
31093
31094 @Test
31095 public void testGetSetInitialMxy() throws WrongSizeException, LockedException {
31096 final KnownPositionAccelerometerCalibrator calibrator =
31097 new KnownPositionAccelerometerCalibrator();
31098
31099 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31100
31101
31102 final Matrix ma = generateMaGeneral();
31103 final double mxy = ma.getElementAt(0, 1);
31104
31105 calibrator.setInitialMxy(mxy);
31106
31107
31108 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31109 }
31110
31111 @Test
31112 public void testGetSetInitialMxz() throws WrongSizeException, LockedException {
31113 final KnownPositionAccelerometerCalibrator calibrator =
31114 new KnownPositionAccelerometerCalibrator();
31115
31116 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31117
31118
31119 final Matrix ma = generateMaGeneral();
31120 final double mxz = ma.getElementAt(0, 2);
31121
31122 calibrator.setInitialMxz(mxz);
31123
31124
31125 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31126 }
31127
31128 @Test
31129 public void testGetSetInitialMyx() throws WrongSizeException, LockedException {
31130 final KnownPositionAccelerometerCalibrator calibrator =
31131 new KnownPositionAccelerometerCalibrator();
31132
31133 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31134
31135
31136 final Matrix ma = generateMaGeneral();
31137 final double myx = ma.getElementAt(1, 0);
31138
31139 calibrator.setInitialMyx(myx);
31140
31141
31142 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31143 }
31144
31145 @Test
31146 public void testGetSetInitialMyz() throws WrongSizeException, LockedException {
31147 final KnownPositionAccelerometerCalibrator calibrator =
31148 new KnownPositionAccelerometerCalibrator();
31149
31150 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31151
31152
31153 final Matrix ma = generateMaGeneral();
31154 final double myz = ma.getElementAt(1, 2);
31155
31156 calibrator.setInitialMyz(myz);
31157
31158
31159 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31160 }
31161
31162 @Test
31163 public void testGetSetInitialMzx() throws WrongSizeException, LockedException {
31164 final KnownPositionAccelerometerCalibrator calibrator =
31165 new KnownPositionAccelerometerCalibrator();
31166
31167 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31168
31169
31170 final Matrix ma = generateMaGeneral();
31171 final double mzx = ma.getElementAt(2, 0);
31172
31173 calibrator.setInitialMzx(mzx);
31174
31175
31176 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31177 }
31178
31179 @Test
31180 public void testGetSetInitialMzy() throws WrongSizeException, LockedException {
31181 final KnownPositionAccelerometerCalibrator calibrator =
31182 new KnownPositionAccelerometerCalibrator();
31183
31184 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31185
31186
31187 final Matrix ma = generateMaGeneral();
31188 final double mzy = ma.getElementAt(2, 1);
31189
31190 calibrator.setInitialMzy(mzy);
31191
31192
31193 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31194 }
31195
31196 @Test
31197 public void testSetInitialScalingFactors()
31198 throws WrongSizeException, LockedException {
31199
31200 final KnownPositionAccelerometerCalibrator calibrator =
31201 new KnownPositionAccelerometerCalibrator();
31202
31203
31204 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31205 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31206 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31207
31208
31209 final Matrix ma = generateMaGeneral();
31210 final double sx = ma.getElementAt(0, 0);
31211 final double sy = ma.getElementAt(1, 1);
31212 final double sz = ma.getElementAt(2, 2);
31213
31214 calibrator.setInitialScalingFactors(sx, sy, sz);
31215
31216
31217 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31218 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31219 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31220 }
31221
31222 @Test
31223 public void testSetInitialCrossCouplingErrors()
31224 throws WrongSizeException, LockedException {
31225
31226 final KnownPositionAccelerometerCalibrator calibrator =
31227 new KnownPositionAccelerometerCalibrator();
31228
31229
31230 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31231 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31232 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31233 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31234 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31235 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31236
31237
31238 final Matrix ma = generateMaGeneral();
31239 final double mxy = ma.getElementAt(0, 1);
31240 final double mxz = ma.getElementAt(0, 2);
31241 final double myx = ma.getElementAt(1, 0);
31242 final double myz = ma.getElementAt(1, 2);
31243 final double mzx = ma.getElementAt(2, 0);
31244 final double mzy = ma.getElementAt(2, 1);
31245
31246 calibrator.setInitialCrossCouplingErrors(mxy, mxz, myx, myz, mzx, mzy);
31247
31248
31249 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31250 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31251 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31252 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31253 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31254 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31255 }
31256
31257 @Test
31258 public void testSetInitialScalingFactorsAndCrossCouplingErrors()
31259 throws WrongSizeException, LockedException {
31260
31261 final KnownPositionAccelerometerCalibrator calibrator =
31262 new KnownPositionAccelerometerCalibrator();
31263
31264
31265 assertEquals(calibrator.getInitialSx(), 0.0, 0.0);
31266 assertEquals(calibrator.getInitialSy(), 0.0, 0.0);
31267 assertEquals(calibrator.getInitialSz(), 0.0, 0.0);
31268 assertEquals(calibrator.getInitialMxy(), 0.0, 0.0);
31269 assertEquals(calibrator.getInitialMxz(), 0.0, 0.0);
31270 assertEquals(calibrator.getInitialMyx(), 0.0, 0.0);
31271 assertEquals(calibrator.getInitialMyz(), 0.0, 0.0);
31272 assertEquals(calibrator.getInitialMzx(), 0.0, 0.0);
31273 assertEquals(calibrator.getInitialMzy(), 0.0, 0.0);
31274
31275
31276 final Matrix ma = generateMaGeneral();
31277 final double sx = ma.getElementAt(0, 0);
31278 final double sy = ma.getElementAt(1, 1);
31279 final double sz = ma.getElementAt(2, 2);
31280 final double mxy = ma.getElementAt(0, 1);
31281 final double mxz = ma.getElementAt(0, 2);
31282 final double myx = ma.getElementAt(1, 0);
31283 final double myz = ma.getElementAt(1, 2);
31284 final double mzx = ma.getElementAt(2, 0);
31285 final double mzy = ma.getElementAt(2, 1);
31286
31287 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(sx, sy, sz,
31288 mxy, mxz, myx, myz, mzx, mzy);
31289
31290
31291 assertEquals(calibrator.getInitialSx(), sx, 0.0);
31292 assertEquals(calibrator.getInitialSy(), sy, 0.0);
31293 assertEquals(calibrator.getInitialSz(), sz, 0.0);
31294 assertEquals(calibrator.getInitialMxy(), mxy, 0.0);
31295 assertEquals(calibrator.getInitialMxz(), mxz, 0.0);
31296 assertEquals(calibrator.getInitialMyx(), myx, 0.0);
31297 assertEquals(calibrator.getInitialMyz(), myz, 0.0);
31298 assertEquals(calibrator.getInitialMzx(), mzx, 0.0);
31299 assertEquals(calibrator.getInitialMzy(), mzy, 0.0);
31300 }
31301
31302 @Test
31303 public void testGetSetInitialBias() throws LockedException {
31304 final KnownPositionAccelerometerCalibrator calibrator =
31305 new KnownPositionAccelerometerCalibrator();
31306
31307
31308 final double[] bias1 = calibrator.getInitialBias();
31309 assertArrayEquals(bias1, new double[3], 0.0);
31310
31311
31312 final Matrix ba = generateBa();
31313 final double[] bias2 = ba.getBuffer();
31314 calibrator.setInitialBias(bias2);
31315
31316
31317 final double[] bias3 = calibrator.getInitialBias();
31318 final double[] bias4 = new double[3];
31319 calibrator.getInitialBias(bias4);
31320
31321 assertArrayEquals(bias2, bias3, 0.0);
31322 assertArrayEquals(bias2, bias4, 0.0);
31323
31324
31325 try {
31326 calibrator.getInitialBias(new double[1]);
31327 fail("IllegalArgumentException expected but not thrown");
31328 } catch (final IllegalArgumentException ignore) {
31329 }
31330 try {
31331 calibrator.setInitialBias(new double[1]);
31332 fail("IllegalArgumentException expected but not thrown");
31333 } catch (final IllegalArgumentException ignore) {
31334 }
31335 }
31336
31337 @Test
31338 public void testGetSetInitialBiasAsMatrix() throws WrongSizeException,
31339 LockedException {
31340
31341 final KnownPositionAccelerometerCalibrator calibrator =
31342 new KnownPositionAccelerometerCalibrator();
31343
31344
31345 final Matrix bias1 = calibrator.getInitialBiasAsMatrix();
31346 assertEquals(bias1, new Matrix(3, 1));
31347
31348
31349 final Matrix bias2 = generateBa();
31350 calibrator.setInitialBias(bias2);
31351
31352
31353 final Matrix bias3 = calibrator.getInitialBiasAsMatrix();
31354 final Matrix bias4 = new Matrix(3, 1);
31355 calibrator.getInitialBiasAsMatrix(bias4);
31356
31357 assertEquals(bias2, bias3);
31358 assertEquals(bias2, bias4);
31359
31360
31361 try {
31362 calibrator.getInitialBiasAsMatrix(new Matrix(1, 1));
31363 fail("IllegalArgumentException expected but not thrown");
31364 } catch (final IllegalArgumentException ignore) {
31365 }
31366 try {
31367 calibrator.getInitialBiasAsMatrix(new Matrix(3, 3));
31368 fail("IllegalArgumentException expected but not thrown");
31369 } catch (final IllegalArgumentException ignore) {
31370 }
31371 try {
31372 calibrator.setInitialBias(new Matrix(1, 1));
31373 fail("IllegalArgumentException expected but not thrown");
31374 } catch (final IllegalArgumentException ignore) {
31375 }
31376 try {
31377 calibrator.setInitialBias(new Matrix(3, 3));
31378 fail("IllegalArgumentException expected but not thrown");
31379 } catch (final IllegalArgumentException ignore) {
31380 }
31381 }
31382
31383 @Test
31384 public void testGetSetInitialMa() throws WrongSizeException, LockedException {
31385 final KnownPositionAccelerometerCalibrator calibrator =
31386 new KnownPositionAccelerometerCalibrator();
31387
31388
31389 final Matrix ma1 = calibrator.getInitialMa();
31390 assertEquals(ma1, new Matrix(3, 3));
31391
31392
31393 final Matrix ma2 = generateMaGeneral();
31394 calibrator.setInitialMa(ma2);
31395
31396
31397 final Matrix ma3 = calibrator.getInitialMa();
31398 final Matrix ma4 = new Matrix(3, 3);
31399 calibrator.getInitialMa(ma4);
31400
31401 assertEquals(ma2, ma3);
31402 assertEquals(ma2, ma4);
31403
31404
31405 try {
31406 calibrator.getInitialMa(new Matrix(1, 3));
31407 fail("IllegalArgumentException expected but not thrown");
31408 } catch (final IllegalArgumentException ignore) {
31409 }
31410 try {
31411 calibrator.getInitialMa(new Matrix(3, 1));
31412 fail("IllegalArgumentException expected but not thrown");
31413 } catch (final IllegalArgumentException ignore) {
31414 }
31415 try {
31416 calibrator.setInitialMa(new Matrix(1, 3));
31417 fail("IllegalArgumentException expected but not thrown");
31418 } catch (final IllegalArgumentException ignore) {
31419 }
31420 try {
31421 calibrator.setInitialMa(new Matrix(3, 1));
31422 fail("IllegalArgumentException expected but not thrown");
31423 } catch (final IllegalArgumentException ignore) {
31424 }
31425 }
31426
31427 @Test
31428 public void testGetSetEcefPosition() throws LockedException {
31429 final KnownPositionAccelerometerCalibrator calibrator =
31430 new KnownPositionAccelerometerCalibrator();
31431
31432
31433 assertNull(calibrator.getEcefPosition());
31434
31435
31436 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31437 final double latitude = Math.toRadians(
31438 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31439 final double longitude = Math.toRadians(
31440 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31441 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31442 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31443 final NEDVelocity nedVelocity = new NEDVelocity();
31444 final ECEFPosition ecefPosition = new ECEFPosition();
31445 final ECEFVelocity ecefVelocity = new ECEFVelocity();
31446 NEDtoECEFPositionVelocityConverter.convertNEDtoECEF(nedPosition, nedVelocity,
31447 ecefPosition, ecefVelocity);
31448
31449 calibrator.setPosition(ecefPosition);
31450
31451
31452 assertSame(calibrator.getEcefPosition(), ecefPosition);
31453 }
31454
31455 @Test
31456 public void testGetSetNedPosition() throws LockedException {
31457 final KnownPositionAccelerometerCalibrator calibrator =
31458 new KnownPositionAccelerometerCalibrator();
31459
31460
31461 assertNull(calibrator.getNedPosition());
31462 assertFalse(calibrator.getNedPosition(null));
31463
31464
31465 final UniformRandomizer randomizer = new UniformRandomizer(new Random());
31466 final double latitude = Math.toRadians(
31467 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31468 final double longitude = Math.toRadians(
31469 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31470 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31471 final NEDPosition nedPosition1 = new NEDPosition(latitude, longitude, height);
31472
31473 calibrator.setPosition(nedPosition1);
31474
31475
31476 final NEDPosition nedPosition2 = calibrator.getNedPosition();
31477 final NEDPosition nedPosition3 = new NEDPosition();
31478 calibrator.getNedPosition(nedPosition3);
31479
31480 assertTrue(nedPosition1.equals(nedPosition2, ABSOLUTE_ERROR));
31481 assertTrue(nedPosition1.equals(nedPosition3, ABSOLUTE_ERROR));
31482 }
31483
31484 @Test
31485 public void testGetSetMeasurements() throws LockedException {
31486 final KnownPositionAccelerometerCalibrator calibrator =
31487 new KnownPositionAccelerometerCalibrator();
31488
31489
31490 assertNull(calibrator.getMeasurements());
31491
31492
31493 final Collection<StandardDeviationBodyKinematics> measurements =
31494 Collections.emptyList();
31495 calibrator.setMeasurements(measurements);
31496
31497
31498 assertSame(calibrator.getMeasurements(), measurements);
31499 }
31500
31501 @Test
31502 public void testIsSetCommonAxisUsed() throws LockedException {
31503 final KnownPositionAccelerometerCalibrator calibrator =
31504 new KnownPositionAccelerometerCalibrator();
31505
31506
31507 assertFalse(calibrator.isCommonAxisUsed());
31508
31509
31510 calibrator.setCommonAxisUsed(true);
31511
31512
31513 assertTrue(calibrator.isCommonAxisUsed());
31514 }
31515
31516 @Test
31517 public void testGetSetListener() throws LockedException {
31518 final KnownPositionAccelerometerCalibrator calibrator =
31519 new KnownPositionAccelerometerCalibrator();
31520
31521
31522 assertNull(calibrator.getListener());
31523
31524
31525 calibrator.setListener(this);
31526
31527
31528 assertSame(calibrator.getListener(), this);
31529 }
31530
31531 @Test
31532 public void testIsReady() throws LockedException {
31533 final KnownPositionAccelerometerCalibrator calibrator =
31534 new KnownPositionAccelerometerCalibrator();
31535
31536
31537 assertFalse(calibrator.isReady());
31538
31539
31540
31541 final Collection<StandardDeviationBodyKinematics> measurements1 =
31542 Collections.emptyList();
31543 calibrator.setMeasurements(measurements1);
31544
31545
31546 assertFalse(calibrator.isReady());
31547
31548
31549
31550 calibrator.setCommonAxisUsed(false);
31551
31552 final List<StandardDeviationBodyKinematics> measurements2 = new ArrayList<>();
31553 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31554 measurements2.add(new StandardDeviationBodyKinematics());
31555 }
31556 calibrator.setMeasurements(measurements2);
31557
31558
31559 assertFalse(calibrator.isReady());
31560
31561
31562
31563 final ECEFPosition position = new ECEFPosition();
31564 calibrator.setPosition(position);
31565
31566 assertTrue(calibrator.isReady());
31567
31568
31569
31570 measurements2.clear();
31571 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31572 measurements2.add(new StandardDeviationBodyKinematics());
31573 }
31574 calibrator.setMeasurements(measurements2);
31575
31576
31577 assertFalse(calibrator.isReady());
31578
31579 calibrator.setCommonAxisUsed(true);
31580
31581 assertTrue(calibrator.isReady());
31582 }
31583
31584 @Test
31585 public void testCalibrateForGeneralCaseWithMinimumMeasuresAndNoNoise()
31586 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31587 LockedException, NotReadyException {
31588
31589 int numValid = 0;
31590 for (int t = 0; t < TIMES; t++) {
31591 final Matrix ba = generateBa();
31592 final Matrix bg = generateBg();
31593 final Matrix ma = generateMaGeneral();
31594 final Matrix mg = generateMg();
31595 final Matrix gg = generateGg();
31596
31597
31598 final double accelNoiseRootPSD = 0.0;
31599 final double gyroNoiseRootPSD = 0.0;
31600 final double accelQuantLevel = 0.0;
31601 final double gyroQuantLevel = 0.0;
31602
31603 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31604 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31605
31606 final Random random = new Random();
31607 final UniformRandomizer randomizer = new UniformRandomizer(random);
31608 final double latitude = Math.toRadians(
31609 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31610 final double longitude = Math.toRadians(
31611 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31612 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31613 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31614
31615 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31616 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31617 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31618
31619 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31620 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_GENERAL; i++) {
31621 final double roll = Math.toRadians(
31622 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31623 final double pitch = Math.toRadians(
31624 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31625 final double yaw = Math.toRadians(
31626 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31627 final CoordinateTransformation nedC = new CoordinateTransformation(
31628 roll, pitch, yaw, FrameType.BODY_FRAME,
31629 FrameType.LOCAL_NAVIGATION_FRAME);
31630
31631 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31632 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31633 .convertNEDtoECEFAndReturnNew(nedFrame);
31634
31635
31636
31637 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31638 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31639 ecefFrame);
31640
31641
31642
31643 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31644 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31645
31646 final StandardDeviationBodyKinematics measurement =
31647 new StandardDeviationBodyKinematics(measuredKinematics,
31648 specificForceStandardDeviation,
31649 angularRateStandardDeviation);
31650 measurements.add(measurement);
31651 }
31652
31653
31654
31655 final KnownPositionAccelerometerCalibrator calibrator =
31656 new KnownPositionAccelerometerCalibrator(nedPosition,
31657 measurements, false, ba, ma, this);
31658
31659
31660 reset();
31661 assertTrue(calibrator.isReady());
31662 assertFalse(calibrator.isRunning());
31663 assertEquals(mCalibrateStart, 0);
31664 assertEquals(mCalibrateEnd, 0);
31665
31666 try {
31667 calibrator.calibrate();
31668 } catch (final CalibrationException e) {
31669 continue;
31670 }
31671
31672
31673 assertTrue(calibrator.isReady());
31674 assertFalse(calibrator.isRunning());
31675 assertEquals(mCalibrateStart, 1);
31676 assertEquals(mCalibrateEnd, 1);
31677
31678 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31679 final Matrix estimatedMa = calibrator.getEstimatedMa();
31680
31681 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31682 continue;
31683 }
31684 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31685 continue;
31686 }
31687
31688 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31689 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31690
31691 numValid++;
31692
31693 break;
31694 }
31695
31696 assertTrue(numValid > 0);
31697 }
31698
31699 @Test
31700 public void testCalibrateForGeneralCaseWithLargeNumberOfMeasurementsAndNoise()
31701 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31702 LockedException, NotReadyException {
31703
31704 int numValid = 0;
31705 for (int t = 0; t < TIMES; t++) {
31706 final Matrix ba = generateBa();
31707 final Matrix bg = generateBg();
31708 final Matrix ma = generateMaGeneral();
31709 final Matrix mg = generateMg();
31710 final Matrix gg = generateGg();
31711 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31712 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31713 final double accelQuantLevel = 0.0;
31714 final double gyroQuantLevel = 0.0;
31715
31716 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31717 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31718
31719 final Random random = new Random();
31720 final UniformRandomizer randomizer = new UniformRandomizer(random);
31721 final double latitude = Math.toRadians(
31722 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31723 final double longitude = Math.toRadians(
31724 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31725 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31726 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31727
31728 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31729 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31730 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31731
31732 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31733 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31734 final double roll = Math.toRadians(
31735 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31736 final double pitch = Math.toRadians(
31737 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31738 final double yaw = Math.toRadians(
31739 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31740 final CoordinateTransformation nedC = new CoordinateTransformation(
31741 roll, pitch, yaw, FrameType.BODY_FRAME,
31742 FrameType.LOCAL_NAVIGATION_FRAME);
31743
31744 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31745 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31746 .convertNEDtoECEFAndReturnNew(nedFrame);
31747
31748
31749
31750 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31751 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31752 ecefFrame);
31753
31754
31755
31756 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31757 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31758
31759 final StandardDeviationBodyKinematics measurement =
31760 new StandardDeviationBodyKinematics(measuredKinematics,
31761 specificForceStandardDeviation,
31762 angularRateStandardDeviation);
31763 measurements.add(measurement);
31764 }
31765
31766
31767
31768 final KnownPositionAccelerometerCalibrator calibrator =
31769 new KnownPositionAccelerometerCalibrator(nedPosition,
31770 measurements, false, this);
31771
31772
31773 reset();
31774 assertTrue(calibrator.isReady());
31775 assertFalse(calibrator.isRunning());
31776 assertEquals(mCalibrateStart, 0);
31777 assertEquals(mCalibrateEnd, 0);
31778
31779 try {
31780 calibrator.calibrate();
31781 } catch (final CalibrationException e) {
31782 continue;
31783 }
31784
31785
31786 assertTrue(calibrator.isReady());
31787 assertFalse(calibrator.isRunning());
31788 assertEquals(mCalibrateStart, 1);
31789 assertEquals(mCalibrateEnd, 1);
31790
31791 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31792 final Matrix estimatedMa = calibrator.getEstimatedMa();
31793
31794 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
31795 continue;
31796 }
31797 if (!ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR)) {
31798 continue;
31799 }
31800
31801 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
31802 assertTrue(ma.equals(estimatedMa, 6.0 * LARGE_ABSOLUTE_ERROR));
31803
31804 numValid++;
31805
31806 break;
31807 }
31808
31809 assertTrue(numValid > 0);
31810 }
31811
31812 @Test
31813 public void testCalibrateForCommonAxisCaseWithMinimumMeasuresAndNoNoise()
31814 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31815 LockedException, NotReadyException {
31816
31817 int numValid = 0;
31818 for (int t = 0; t < TIMES; t++) {
31819 final Matrix ba = generateBa();
31820 final Matrix bg = generateBg();
31821 final Matrix ma = generateMaCommonAxis();
31822 final Matrix mg = generateMg();
31823 final Matrix gg = generateGg();
31824
31825
31826 final double accelNoiseRootPSD = 0.0;
31827 final double gyroNoiseRootPSD = 0.0;
31828 final double accelQuantLevel = 0.0;
31829 final double gyroQuantLevel = 0.0;
31830
31831 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31832 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31833
31834 final Random random = new Random();
31835 final UniformRandomizer randomizer = new UniformRandomizer(random);
31836 final double latitude = Math.toRadians(
31837 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31838 final double longitude = Math.toRadians(
31839 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31840 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31841 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31842
31843 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31844 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31845 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31846
31847 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31848 for (int i = 0; i < KnownPositionAccelerometerCalibrator.MINIMUM_MEASUREMENTS_COMON_Z_AXIS; i++) {
31849 final double roll = Math.toRadians(
31850 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31851 final double pitch = Math.toRadians(
31852 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31853 final double yaw = Math.toRadians(
31854 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31855 final CoordinateTransformation nedC = new CoordinateTransformation(
31856 roll, pitch, yaw, FrameType.BODY_FRAME,
31857 FrameType.LOCAL_NAVIGATION_FRAME);
31858
31859 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31860 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31861 .convertNEDtoECEFAndReturnNew(nedFrame);
31862
31863
31864
31865 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31866 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31867 ecefFrame);
31868
31869
31870
31871 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31872 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31873
31874 final StandardDeviationBodyKinematics measurement =
31875 new StandardDeviationBodyKinematics(measuredKinematics,
31876 specificForceStandardDeviation,
31877 angularRateStandardDeviation);
31878 measurements.add(measurement);
31879 }
31880
31881
31882
31883 final KnownPositionAccelerometerCalibrator calibrator =
31884 new KnownPositionAccelerometerCalibrator(nedPosition,
31885 measurements, true, ba, ma, this);
31886
31887
31888 reset();
31889 assertTrue(calibrator.isReady());
31890 assertFalse(calibrator.isRunning());
31891 assertEquals(mCalibrateStart, 0);
31892 assertEquals(mCalibrateEnd, 0);
31893
31894 try {
31895 calibrator.calibrate();
31896 } catch (final CalibrationException e) {
31897 continue;
31898 }
31899
31900
31901 assertTrue(calibrator.isReady());
31902 assertFalse(calibrator.isRunning());
31903 assertEquals(mCalibrateStart, 1);
31904 assertEquals(mCalibrateEnd, 1);
31905
31906 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
31907 final Matrix estimatedMa = calibrator.getEstimatedMa();
31908
31909 if (!ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR)) {
31910 continue;
31911 }
31912 if (!ma.equals(estimatedMa, ABSOLUTE_ERROR)) {
31913 continue;
31914 }
31915
31916 assertTrue(ba.equals(estimatedBa, LARGE_ABSOLUTE_ERROR));
31917 assertTrue(ma.equals(estimatedMa, ABSOLUTE_ERROR));
31918
31919 numValid++;
31920
31921 break;
31922 }
31923
31924 assertTrue(numValid > 0);
31925 }
31926
31927 @Test
31928 public void testCalibrateForCommonAxisCaseWithLargeNumberOfMeasurementsAndNoise()
31929 throws WrongSizeException, InvalidSourceAndDestinationFrameTypeException,
31930 LockedException, NotReadyException {
31931
31932 int numValid = 0;
31933 for (int t = 0; t < TIMES; t++) {
31934 final Matrix ba = generateBa();
31935 final Matrix bg = generateBg();
31936 final Matrix ma = generateMaCommonAxis();
31937 final Matrix mg = generateMg();
31938 final Matrix gg = generateGg();
31939 final double accelNoiseRootPSD = getAccelNoiseRootPSD();
31940 final double gyroNoiseRootPSD = getGyroNoiseRootPSD();
31941 final double accelQuantLevel = 0.0;
31942 final double gyroQuantLevel = 0.0;
31943
31944 final IMUErrors errors = new IMUErrors(ba, bg, ma, mg, gg, accelNoiseRootPSD,
31945 gyroNoiseRootPSD, accelQuantLevel, gyroQuantLevel);
31946
31947 final Random random = new Random();
31948 final UniformRandomizer randomizer = new UniformRandomizer(random);
31949 final double latitude = Math.toRadians(
31950 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
31951 final double longitude = Math.toRadians(
31952 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
31953 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
31954 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
31955
31956 final double sqrtTimeInterval = Math.sqrt(TIME_INTERVAL_SECONDS);
31957 final double specificForceStandardDeviation = getAccelNoiseRootPSD() / sqrtTimeInterval;
31958 final double angularRateStandardDeviation = getGyroNoiseRootPSD() / sqrtTimeInterval;
31959
31960 final List<StandardDeviationBodyKinematics> measurements = new ArrayList<>();
31961 for (int i = 0; i < LARGE_MEASUREMENT_NUMBER; i++) {
31962 final double roll = Math.toRadians(
31963 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31964 final double pitch = Math.toRadians(
31965 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31966 final double yaw = Math.toRadians(
31967 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
31968 final CoordinateTransformation nedC = new CoordinateTransformation(
31969 roll, pitch, yaw, FrameType.BODY_FRAME,
31970 FrameType.LOCAL_NAVIGATION_FRAME);
31971
31972 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
31973 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
31974 .convertNEDtoECEFAndReturnNew(nedFrame);
31975
31976
31977
31978 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
31979 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
31980 ecefFrame);
31981
31982
31983
31984 final BodyKinematics measuredKinematics = BodyKinematicsGenerator
31985 .generate(TIME_INTERVAL_SECONDS, trueKinematics, errors, random);
31986
31987 final StandardDeviationBodyKinematics measurement =
31988 new StandardDeviationBodyKinematics(measuredKinematics,
31989 specificForceStandardDeviation,
31990 angularRateStandardDeviation);
31991 measurements.add(measurement);
31992 }
31993
31994
31995
31996 final KnownPositionAccelerometerCalibrator calibrator =
31997 new KnownPositionAccelerometerCalibrator(nedPosition,
31998 measurements, true, this);
31999
32000
32001 reset();
32002 assertTrue(calibrator.isReady());
32003 assertFalse(calibrator.isRunning());
32004 assertEquals(mCalibrateStart, 0);
32005 assertEquals(mCalibrateEnd, 0);
32006
32007 try {
32008 calibrator.calibrate();
32009 } catch (final CalibrationException e) {
32010 continue;
32011 }
32012
32013
32014 assertTrue(calibrator.isReady());
32015 assertFalse(calibrator.isRunning());
32016 assertEquals(mCalibrateStart, 1);
32017 assertEquals(mCalibrateEnd, 1);
32018
32019 final Matrix estimatedBa = calibrator.getEstimatedBiasesAsMatrix();
32020 final Matrix estimatedMa = calibrator.getEstimatedMa();
32021
32022 if (!ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR)) {
32023 continue;
32024 }
32025 if (!ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR)) {
32026 continue;
32027 }
32028
32029 assertTrue(ba.equals(estimatedBa, VERY_LARGE_ABSOLUTE_ERROR));
32030 assertTrue(ma.equals(estimatedMa, LARGE_ABSOLUTE_ERROR));
32031
32032 numValid++;
32033
32034 break;
32035 }
32036
32037 assertTrue(numValid > 0);
32038 }
32039
32040 @Test
32041 public void testNorms() throws InvalidSourceAndDestinationFrameTypeException {
32042
32043
32044
32045
32046
32047
32048
32049
32050 final Random random = new Random();
32051 final UniformRandomizer randomizer = new UniformRandomizer(random);
32052 final double latitude = Math.toRadians(
32053 randomizer.nextDouble(MIN_LATITUDE_DEGREES, MAX_LATITUDE_DEGREEs));
32054 final double longitude = Math.toRadians(
32055 randomizer.nextDouble(MIN_LONGITUDE_DEGREES, MAX_LONGITUDE_DEGREES));
32056 final double height = randomizer.nextDouble(MIN_HEIGHT, MAX_HEIGHT);
32057 final NEDPosition nedPosition = new NEDPosition(latitude, longitude, height);
32058
32059 final double roll = Math.toRadians(
32060 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32061 final double pitch = Math.toRadians(
32062 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32063 final double yaw = Math.toRadians(
32064 randomizer.nextDouble(MIN_ANGLE_DEGREES, MAX_ANGLE_DEGREES));
32065 final CoordinateTransformation nedC = new CoordinateTransformation(
32066 roll, pitch, yaw, FrameType.BODY_FRAME,
32067 FrameType.LOCAL_NAVIGATION_FRAME);
32068
32069 final NEDFrame nedFrame = new NEDFrame(nedPosition, nedC);
32070 final ECEFFrame ecefFrame = NEDtoECEFFrameConverter
32071 .convertNEDtoECEFAndReturnNew(nedFrame);
32072
32073 final BodyKinematics trueKinematics = ECEFKinematicsEstimator
32074 .estimateKinematicsAndReturnNew(TIME_INTERVAL_SECONDS, ecefFrame,
32075 ecefFrame);
32076
32077 final double fNorm = trueKinematics.getSpecificForceNorm();
32078 final double wNorm = trueKinematics.getAngularRateNorm();
32079
32080 final ECEFGravity gravity = ECEFGravityEstimator.estimateGravityAndReturnNew(ecefFrame);
32081 final double g = gravity.getNorm();
32082
32083 assertEquals(g, fNorm, ABSOLUTE_ERROR);
32084 assertEquals(Constants.EARTH_ROTATION_RATE, wNorm, ABSOLUTE_ERROR);
32085 }
32086
32087 @Override
32088 public void onCalibrateStart(
32089 final KnownPositionAccelerometerCalibrator calibrator) {
32090 checkLocked(calibrator);
32091 mCalibrateStart++;
32092 }
32093
32094 @Override
32095 public void onCalibrateEnd(
32096 final KnownPositionAccelerometerCalibrator calibrator) {
32097 checkLocked(calibrator);
32098 mCalibrateEnd++;
32099 }
32100
32101 private void reset() {
32102 mCalibrateStart = 0;
32103 mCalibrateEnd = 0;
32104 }
32105
32106 private void checkLocked(final KnownPositionAccelerometerCalibrator calibrator) {
32107 assertTrue(calibrator.isRunning());
32108 try {
32109 calibrator.setInitialBiasX(0.0);
32110 fail("LockedException expected but not thrown");
32111 } catch (final LockedException ignore) {
32112 }
32113 try {
32114 calibrator.setInitialBiasY(0.0);
32115 fail("LockedException expected but not thrown");
32116 } catch (final LockedException ignore) {
32117 }
32118 try {
32119 calibrator.setInitialBiasZ(0.0);
32120 fail("LockedException expected but not thrown");
32121 } catch (final LockedException ignore) {
32122 }
32123 try {
32124 calibrator.setInitialBiasX(null);
32125 fail("LockedException expected but not thrown");
32126 } catch (final LockedException ignore) {
32127 }
32128 try {
32129 calibrator.setInitialBiasY(null);
32130 fail("LockedException expected but not thrown");
32131 } catch (final LockedException ignore) {
32132 }
32133 try {
32134 calibrator.setInitialBiasZ(null);
32135 fail("LockedException expected but not thrown");
32136 } catch (final LockedException ignore) {
32137 }
32138 try {
32139 calibrator.setInitialBias(0.0, 0.0, 0.0);
32140 fail("LockedException expected but not thrown");
32141 } catch (final LockedException ignore) {
32142 }
32143 try {
32144 calibrator.setInitialBias(null, null, null);
32145 fail("LockedException expected but not thrown");
32146 } catch (final LockedException ignore) {
32147 }
32148 try {
32149 calibrator.setInitialSx(0.0);
32150 fail("LockedException expected but not thrown");
32151 } catch (final LockedException ignore) {
32152 }
32153 try {
32154 calibrator.setInitialSy(0.0);
32155 fail("LockedException expected but not thrown");
32156 } catch (final LockedException ignore) {
32157 }
32158 try {
32159 calibrator.setInitialSz(0.0);
32160 fail("LockedException expected but not thrown");
32161 } catch (final LockedException ignore) {
32162 }
32163 try {
32164 calibrator.setInitialMxy(0.0);
32165 fail("LockedException expected but not thrown");
32166 } catch (final LockedException ignore) {
32167 }
32168 try {
32169 calibrator.setInitialMxz(0.0);
32170 fail("LockedException expected but not thrown");
32171 } catch (final LockedException ignore) {
32172 }
32173 try {
32174 calibrator.setInitialMyx(0.0);
32175 fail("LockedException expected but not thrown");
32176 } catch (final LockedException ignore) {
32177 }
32178 try {
32179 calibrator.setInitialMyz(0.0);
32180 fail("LockedException expected but not thrown");
32181 } catch (final LockedException ignore) {
32182 }
32183 try {
32184 calibrator.setInitialMzx(0.0);
32185 fail("LockedException expected but not thrown");
32186 } catch (final LockedException ignore) {
32187 }
32188 try {
32189 calibrator.setInitialMzy(0.0);
32190 fail("LockedException expected but not thrown");
32191 } catch (final LockedException ignore) {
32192 }
32193 try {
32194 calibrator.setInitialScalingFactors(0.0, 0.0, 0.0);
32195 fail("LockedException expected but not thrown");
32196 } catch (final LockedException ignore) {
32197 }
32198 try {
32199 calibrator.setInitialCrossCouplingErrors(
32200 0.0, 0.0, 0.0,
32201 0.0, 0.0, 0.0);
32202 fail("LockedException expected but not thrown");
32203 } catch (final LockedException ignore) {
32204 }
32205 try {
32206 calibrator.setInitialScalingFactorsAndCrossCouplingErrors(
32207 0.0, 0.0, 0.0,
32208 0.0, 0.0, 0.0,
32209 0.0, 0.0, 0.0);
32210 fail("LockedException expected but not thrown");
32211 } catch (final LockedException ignore) {
32212 }
32213 try {
32214 calibrator.setInitialBias((double[]) null);
32215 fail("LockedException expected but not thrown");
32216 } catch (final LockedException ignore) {
32217 }
32218 try {
32219 calibrator.setInitialBias((Matrix) null);
32220 fail("LockedException expected but not thrown");
32221 } catch (final LockedException ignore) {
32222 }
32223 try {
32224 calibrator.setInitialMa(null);
32225 fail("LockedException expected but not thrown");
32226 } catch (final LockedException ignore) {
32227 }
32228 try {
32229 calibrator.setPosition((ECEFPosition) null);
32230 fail("LockedException expected but not thrown");
32231 } catch (final LockedException ignore) {
32232 }
32233 try {
32234 calibrator.setPosition((NEDPosition) null);
32235 fail("LockedException expected but not thrown");
32236 } catch (final LockedException ignore) {
32237 }
32238 try {
32239 calibrator.setMeasurements(null);
32240 fail("LockedException expected but not thrown");
32241 } catch (final LockedException ignore) {
32242 }
32243 try {
32244 calibrator.setCommonAxisUsed(true);
32245 fail("LockedException expected but not thrown");
32246 } catch (final LockedException ignore) {
32247 }
32248 try {
32249 calibrator.setListener(this);
32250 fail("LockedException expected but not thrown");
32251 } catch (final LockedException ignore) {
32252 }
32253 try {
32254 calibrator.calibrate();
32255 fail("LockedException expected but not thrown");
32256 } catch (final LockedException ignore) {
32257 } catch (final Exception e) {
32258 fail("LockedException expected but not thrown");
32259 }
32260 }
32261
32262 private Matrix generateBa() {
32263 return Matrix.newFromArray(new double[]{
32264 900 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32265 -1300 * MICRO_G_TO_METERS_PER_SECOND_SQUARED,
32266 800 * MICRO_G_TO_METERS_PER_SECOND_SQUARED});
32267 }
32268
32269 private Matrix generateBg() {
32270 return Matrix.newFromArray(new double[]{
32271 -9 * DEG_TO_RAD / 3600.0,
32272 13 * DEG_TO_RAD / 3600.0,
32273 -8 * DEG_TO_RAD / 3600.0});
32274 }
32275
32276 private Matrix generateMaGeneral() throws WrongSizeException {
32277 final Matrix result = new Matrix(3, 3);
32278 result.fromArray(new double[]{
32279 500e-6, -300e-6, 200e-6,
32280 -150e-6, -600e-6, 250e-6,
32281 -250e-6, 100e-6, 450e-6
32282 }, false);
32283
32284 return result;
32285 }
32286
32287 private Matrix generateMaCommonAxis() throws WrongSizeException {
32288 final Matrix result = new Matrix(3, 3);
32289 result.fromArray(new double[]{
32290 500e-6, -300e-6, 200e-6,
32291 0.0, -600e-6, 250e-6,
32292 0.0, 0.0, 450e-6
32293 }, false);
32294
32295 return result;
32296 }
32297
32298 private Matrix generateMg() throws WrongSizeException {
32299 final Matrix result = new Matrix(3, 3);
32300 result.fromArray(new double[]{
32301 400e-6, -300e-6, 250e-6,
32302 0.0, -300e-6, -150e-6,
32303 0.0, 0.0, -350e-6
32304 }, false);
32305
32306 return result;
32307 }
32308
32309 private Matrix generateGg() throws WrongSizeException {
32310 final Matrix result = new Matrix(3, 3);
32311 final double tmp = DEG_TO_RAD / (3600 * 9.80665);
32312 result.fromArray(new double[]{
32313 0.9 * tmp, -1.1 * tmp, -0.6 * tmp,
32314 -0.5 * tmp, 1.9 * tmp, -1.6 * tmp,
32315 0.3 * tmp, 1.1 * tmp, -1.3 * tmp
32316 }, false);
32317
32318 return result;
32319 }
32320
32321 private double getAccelNoiseRootPSD() {
32322 return 100.0 * MICRO_G_TO_METERS_PER_SECOND_SQUARED;
32323 }
32324
32325 private double getGyroNoiseRootPSD() {
32326 return 0.01 * DEG_TO_RAD / 60.0;
32327 }
32328 }